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ABSTRACT 


This  project  is  concerned  with  the  study  of  the  behavior  of  a 
particular  stochastic  system,  the  magnetic  suspension  system,  under  the 
application  of  the  Sensitivity  Adaptive  Feedback  with  Estimation  Redistribu¬ 
tion  (SAFER)  Control  Algorithm.  Matrix  factorization  techniques  are  used 
in  the  controller  and  estimator  design  for  the  system.  Simulation  results 
indicate  that  the  magnetic  suspension  system  can  operate  satisfactorily  under 
the  SAFER  control  method;  and  factorization  techniques  indeed  enhance  the 
numerical  stability  of  computations.  However,  due  to  the  complex  structure 
of  the  SAFER  control  method,  real  time  application  of  the  algorithm  may 
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CHAPTER  1 
INTRODUCTION 

With  the  advent  of  sophisticated  digital  computers,  it  is  now 
possible  to  implement  more  complex  adaptive  controllers.  Adaptive  controll¬ 
ers  are  appealing  as  they  are  able  to  modify  their  behavior  depending  on 
the  performance  of  the  control  led  process,  thus  eliminating  or  reducing  the 
need  for  manual  tuning  of  the  parameters  of  the  controllers.  One  reason 
for  the  presence  of  these  controllers  in  the  process  industry  is  that  the 
dynamics  of  the  process  or  the  noise  are  changing  due  to  changes  in  produc¬ 
tion  or  wear  of  the  equipment.  The  use  of  adaptive  controllers  takes  these 
changes  into  account  and  arrive  at  an  optimal  control  according  to  a 
prescribed  manner. 

1.1.  Adaptive  Controllers 

Many  adaptive  controllers  are  constructed  by  taking  into  considera¬ 
tion  the  statistical  nature  of  the  fluctuation  of  the  parameters  or  the 
disturbances  acting  on  the  system;  this  class  of  controllers  is  called 
stochastic  adaptive  controllers.  Stochastic  adaptive  controllers  can  further 
be  divided  into  non-dual  and  dual  controllers  according  to  the  structure  of 
the  performance  index  imposed  on  the  system  under  consideration.  If  the 
performance  index  only  takes  into  account  the  previous  measurements  and 
does  not  assume  that  further  information  will  be  available,  then  the  result¬ 
ing  controller  are  called  non-dual.  On  the  other  hand,  the  performance  index 
can  also  be  dependent  on  the  future  observations  and  this  will  result  in  a 


dual  controller.  Hence,  the  minimization  of  a  loss  function  (performance 
index),  when  the  measurement  program  is  incorporated,  several  steps  ahead 
will  give  a  dual  controller  and  it  might  be  worthwhile  for  the  controller 
to  take  some  actions  in  order  to  improve  the  estimates  of  the  fluctuating 
parameters,  which  may  also  be  unknown.  That  is,  the  dual  controller  must 
ensure  good  control  and  good  estimation,  which  in  general  are  contradictory 
since  good  estimation  might  require  large  control  signals  while  good  control 
might  require  that  the  control  signals  to  be  small.  A  dual  controller  thus 
must  compromise  between  these  two  tasks  [16]. 

1.2.  Adaptive  Sensitivity  Controller 

One  method  of  constructing  a  dual  controller  for  an  uncertain 
system  with  unknown  parameters  is  to  design  a  control  algorithm  based  on 
the  information  extracted  from  the  sensitivities  of  the  system  [12,13,14], 
This  method,  the  Sensitivity  Adaptive  Feedback  with  Estimation  Redistribu¬ 
tion  (SAFER)  control  algorithm,  incorporates  a  cost  assignment  for  the 
estimation  effort  and  the  estimation  budget  is  distributed  according  to  the 
accuracy  required  to  achieve  a  given  control  objective.  The  rationale 
underlying  this  method  is  that  parameters  to  which  the  state  of  the  system 
is  more  sensitive  require  more  accurate  estimation  than  those  whose  effect 
on  the  state  is  less  significant.  The  fixed  budget  for  estimation  is  re¬ 
presented  by  a  sensitivity  constraint,  A  suboptimai  scheme  is  then  used  to 
compute  a  control  (which  will  in  turn  influence  the  sensitivity  of  the 
system)  that  will  minimize  the  covariance  of  the  estimate  of  the  unknown 
parameters . 
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L.3.  Thesis  Outline 

In  this  thesis,  a  stochastic  model  for  a  magnetic  ball  suspension 
system  is  studied  and  simulated  using  a  digital  computer.  The  control 
algorithm  used  is  the  previously  mentioned  SAFER  method.  The  objective  of 
the  project  is  to  investigate  the  performance  of  the  magnetic  suspension 
system  under  the  application  of  the  SAFER  control  algorithm.  In  addition, 
matrix  factorization  techniques  are  used  in  some  parts  of  the  controller  and 
estimator  design  to  determine  if  numerical  stability  can  be  enhanced. 

The  actual  non-linear  model  of  the  magnetic  ball  suspension  system 
is  first  linearized,  then  discretized.  A  set  of  feedback  gains  is  then  com¬ 
puted  using  the  SAFER  method.  Finally,  an  extended  Kalman  filter  utilizing 
matrix  factorization  algorithm  is  constructed  for  estimating  the  unknown 
parameters  of  the  model.  Monte  Carlo  simulations  are  then  carried  out  with 
this  closed- loop  system.  Various  results,  comments  and  remarks  on  the  sys¬ 
tem  or  the  method  are  mentioned  in  subsequent  chapters  of  the  thesis. 


CHAPTER  2 


MAGNETIC  BALL  SUSPENSION  MODEL 
AND  PROBLEM  FORMULATION 


The  magnetic  ball  suspension  system  is  studied  since  in  many 
situations  it  is  desirable  to  support  an  object  with  a  magnetic  field;  for 
example,  in  a  wind  tunnel,  effects  of  the  mechanical  structure  (stinger) 
supporting  the  model  under  study  introduce  errors  in  drag  and  lift  measure¬ 
ments.  One  solution  is  naturally  to  use  a  magnetic  field  to  support  the 
model  in  position  [5].  Another  application  of  this  system  is  that  it  serves 
as  a  simplified  model  for  a  magnetically  suspended  vehicle  [9]. 

2.1.  Equation  of  Motion 

The  equation  of  motion  for  the  magnetic  ball  suspension  system  has 

the  form 


m 


.2 

ci 

—  +  mg 


(2.1) 


where  m  is  the  mass  of  the  ball,  g  is  the  gravitational  constant,  h  is  the 
distance  of  the  ball  from  the  coil,  i  is  the  current  in  the  coil  which 
generates  the  electromagnetic  force  and  c  is  a  proportionality  constant 
which  is  dependent  upon  the  particular  configuration  and  construction  of 
the  coil,  number  of  turns  per  centimeter,  for  instance.  The  derivative  in 
the  equation  is  taken  with  respect  to  time,  thus,  the  second  derivative  of  h 
with  respect  to  time  t  will  be  the  acceleration  of  the  ball.  An  illustration 
of  the  configuration  is  shown  in  Figure  1. 


Fig.  1.  Magnecic  Bail  Suspension  System  Configuration 


To  formulate  the  problem  using  the  state  space  approach,  let 


x,  =  h 


dh 

at 


=  x. 


u  =  \ 


then  equation  (2.1) 


can  be  transformed 


into 


the 


following 


second  order  sys¬ 


tem: 


6 


=  x 


2 


x 


2 


(2.2) 

(2.3) 


2.2,  Linearization 

Since  the  control  algorithm  to  be  used  applies  to  a  linear  system, 
it  is  necessary  to  linearize  the  present  non-linear  system  using  first  order 
perturbation  techniques,  that  is,  given  the  non-linear  system 

x  =  f(x,u)  (2.4) 


we  linearize  it  around  an  equilibrium  position  and  obtain 


6x=||6x+|^5u  =  cSx  +  d5u 

Oa  OU 


(2.5) 


where 


C 


of 

dx 


D 


of 

OU 


Before  computing  the  various  partial  derivatives,  an  equilibrium 

state  of  the  system  has  to  be  evaluated.  This  is  done  by  setting,  in  the 

•  _  _ 

original  non-linear  system,  x=f(x,u)  =0,  where  x  and  u  are  equilibrium  values. 

Setting  equation  (2.2)  and  (2.3)  equal  to  zero,  we  obtain  the 
equilibrium  values  for  x^  and  x^ : 

*2  =  Q  (2.6) 

-2 


x 


1 


CL 

mg 


(2.7) 
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where  i  is  the  bias  current  in  the  coil. 

Computing  the  various  partial  derivatives,  the  following  linearized 
system  is  obtained: 


6x^  =  6X2 

c  .  ci2  c  2cT  c 

6x„  =  - r  ax. - - —  Ou 

2  _  2  1  mx, 

mx  1 


Substituting  equation  (2.7)  into  (2.9),  we  obtain 


6x2  =  6x^  -  6u 

ci  i 


Following  the  notation  in  equation  (2.5),  we  have 


0 

1 

D  = 

0 

2 

mg_ 

0 

-2 

CL 

(2.8) 

(2.9) 


(2.10) 


To  simplify  the  notation,  we  will  denote  hereafter,  without  ambiguity,  5x 
simply  by  x,  6u  by  u,  that  is,  equation  (2.10)  will  appear  in  the  form 
x  =  Cx  +  Du. 

To  attain  a  more  realistic  model  for  the  system,  we  will  take  into 
consideration  the  effect  of  disturbances,  such  as  drag  force  and  transducer 
measurement  error,  by  appending  a  noise  term  w  into  equation  (2.10),  which 
is  the  linearized  version  of  the  original  system.  Hence,  the  following 
linearized  system  with  noise  is  obtained 

x  =  Cx  +  Du  +  Fw  (2.11) 


8 


where 


2 . 3.  Discretization 


As  the  control  algorithm  to  be  applied  deals  with  discrete  system, 
it  is  necessary  to  discretize  the  present  linearized  system  (2.11).  In 
order  to  simplify  computations,  discretization  by  the  method  of  finite 
difference  is  used.  That  is,  we  will  approximate  x  by  (x^+^  -  x,^)/T,  where 
T  is  the  sampling  period.  Applying  this  technique  to  equation  (2.11),  we 
obtain 


\+i-<I+CT>*k+DT\+rT“k 


or 


xk+i=AXk  +  Bu,.4-Ew, 


‘k  k 


where  I  is  the  identity  matrix  and  B  = 


0 

1  0 

-2gT 

a 

1  2 

i 

A  = 


1  T 

1  T 

3  1  j 

—2 

i  : 

cx 

—  — 

(2.12) 


The  two  constants  3^  and  3^  are  bhe  unknown  parameters  of  the  discrete 
stochastic  system  of  equation  (2.12).  The  numerical  values  of  the  various 
variables  in  the  model  are  listed  in  Table  1.  This  discrete  stochastic  system 


can  be  modelled  as  illustrated  in  the  block  diagram  given  in  Figure  2 


Table  1 

Numerical  values  of  the  variables  in  the  model  and  the 
corresponding  values  of  the  unknown  parameters 

m  =  0.05  Kg. 

2 

g  =  10.  meters/sc'.ond- 

2 

c  *  0.4  Newton-meter/ampere 
i  =  0.5  ampere 
T  =  0.01  second 
=  0.2  meter 

2 

3^  =  0.5  second/ampere 

=  -0.4  meter/second-ampere 
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CHAPTER  3 
CONTROLLER  DESIGN 

3.1.  Sensitivity  Feedback  Controller 


The  use  of  sensitivity  functions  in  control  systems  to  attain  a 
more  stable  design  has  been  well  investigated  [6],  In  this  thesis,  the 
control  algorithm  to  be  applied  will  utilize  the  sensitivity  functions  in 
such  a  way  that  a  fixed  budget  of  effort  for  estimation  of  unknown  para¬ 
meters  will  be  distributed  rationally  to  certain  states  of  the  system 
based  on  information  extracted  from  the  sensitivity  functions.  In  this 
chapter  we  apply  the  SAFER  control  method  in  references  [12,13,14]  using 
the  notation  found  therein.  We  use  the  U-D  factorization  method  [2,3]  to 
improve  the  numerical  stability  of  the  computations. 

3,1.1.  Problem  Formulation  and  Solution 

Given  the  stochastic  model 

+  Ew. 
k 


Vi  =  Axk  +  Buk 


where  A,  B,  E  are  defined  previously  in  equation  (2.12)  and  an  observation 
equation 


c 


k 


x 

k 


(3.1) 


The  model  considered  here  is  a  second  order  system  with  scalar 

control  and  scalar  noise.  The  noise  w^  is  assumed  to  be  a  random  sequence 

with  zero  mean  and  variance  V  .  The  state  and  the  disturbance  at  the  same 

w 


instant  are  statistically  independent.  The  entries  of  A  and  B  are  considered 
independent.  The  performance  index  to  be  minimized  is; 


N  +  v-1 
o 


h  ■  EttJ»  + 


subject  to 

N  +v-l 

o  N 

J2  =  Eik=N  P’(k)W  °P(k)j  ^  r 
o 

where  El.}  denotes  taking  expectation,  v  is  the  number  of  stages  in  the 
optimization  procedure,  and 


P(k)  =  (P[(k),P^(k))'  =/ 


( 

dXl(k) 

> 

dX]_(k) 

\ 

d6l 

3G2 

< 

dx2(k) 

3*2 (k) 

00  . 

_  1  _ 

9 

*2  _ 

J 

where  P^,  P^  are  designed  to  closely  approximate  the  state  sensitivities 
by  having  them  satisfy  the  following  equations: 

P^k+1)  =  (AQi  -  B^K^k)^  +  [A  -  BK^(k)]  P^(k) 


2 

P2(k+1)  =  (A32  -B02KL(k))xk+  [A  -  BKL(k)]P2(k) 


(3.2) 


-  i5lB8  K2iWPt(k) 
2 


(3.3) 


where  AQ .  and  B-.(j=l,2)  are  the  derivatives  of  A  and  B  with  respect  to  s., 

bj  Cj  j 
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that  is: 


0 

0 

0 

A,  = 

B,  = 

91 

1 

0 

e| 

0 

fo 

«1 

r°i 

0  0 


(3.4) 


(3.5) 


and  A,  B  are  the  Latest  estimates  of  A  and  B  respectively.  @  is  a 
component  of  the  vector  0  which  is  formed  by  the  unknown  entries  of  A  and 
B  taken  by  row,  that  is,  for  the  magnetic  ball  suspension  system: 


ei 

2  -2 
mg  T/(ci  ) 

0.5 

_92_ 

-2gT/I 

-0.4 

(3.6) 


The  matrices  K^(k)  and  Kj (k)  in  equation  (3.2)  and  (3.3)  are  to  be  found 
from  the  sensitivity  control  algorithm,  and  the  control  signal  will  be  in 
the  form 


u 


k 


(3.7) 


The  matrix  Q  is  positive  semidefinite  while  R  is  positive  definite.  The 

N 

unknown  diagonal  matrix  W  °  is  to  be  chosen  by  the  designer  at  time  N  and 

No  .N0,  ° 

satisfies  W^0  a  £•  a  0  for  1*1, 2,3,4,  and  tracetW  }  =1.  The  non-negative 

number  £  is  the  minimum  relative  weight.  It  is  the  choice  of  this  weighting 
N 

matrix  W  that  enables  the  distribution  of  the  estimation  effort  to  be 
done  rationally. 

Furnished  with  all  the  previous  information,  the  original  problem 


can  be  restaced  as  follows: 


Consider  Che  augmented  system 


’k+1  =  *k  \  +  \  Uk  +  fWR 


(3.8) 


with  perfect  state  information.  The  performance  index  to  be  minimized 

N 

o 

with  respect  to  u,  and  W  is: 


where 


N  +v-l 

o 


<{A!k+“lRV} 


(3.9) 


Ak  = 


Ad  -B3  K^k)  A  -  BKl  (k> -Be  K21(k) 


Ae2  ‘  Be2Ki(k) 


•Be2K2i(k) 


-BelK22(k) 

A  -  BK^k)^  K22(k) 


Bk  = 


“  B 

~E~ 

0 

0 

Q 

0 

o 

r  = 

0 

\  = 

0 

0 

0 

0 

_  — 

0 

(3.10) 


3.1.2,  Suboptimal  Solution 


Since  the  exact  optimal  solution  of  equation  (3.9)  subject  to 
equation  (3.8)  is  extremely  involved,  a  suboptimal  solution  which  is  of 
the  feedback  form  will  be  sought  instead.  Before  going  into  the  procedure 
for  finding  this  suboptimal  solution,  a  theorem  found  in  references  [12,13] 
will  be  stated  since  it  will  be  used  during  the  solution  process.  The 


L5 


theorem  shows  that  for  a  fixed  W  and  given  A^  and  B  ,  if  equation 

“  o  “o 

(3.13)  admits  a  solution,  the  control  u^  that  minimizes  J  in  equation 

o 

(3.9)  is 


N  N  N  .  N  N 

u  =  -(R  +  B«  °PN°  P  +iB~  0) 

o  OOO  0  0  0  *o 


(3.11) 


where  P.  is  the  solution  to 
1 


N  N  N  N 

O  -  ~ .  O  0-0 

P.  =  Q .  +  A  (K . ) P .  .A.  (K.) 
l  i  11  1+1  11 


N 


N  N 
o-o 


N  N  N  N 

~  o  o  ~  o  -l  o 


N  N 
o~o 


-ca:  iw,  )•(««;  p1+1bi}  (a:  (Kt)p1+1B.  >' 


N 

P..  ,  i  =  QM  ,  ,  i  i  =N  , . .  .  ,N  +  v-2 
N  +v-l  N  +v-l  o  o 

o  o 


(3.12) 


and 


N  N  N  ,  N  N  N 

Ki  =  'R+B;0pi0+A°)  ^°^pi>7)' 


where 


K  =0  i  =N  ,  . .  .  ,N  +  v-2 

Jl  +  V- 1  o  o 

o 


(3.13) 


K.  =  [K,(i)  Ka(i)  K„(i)] 
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- A  - 

%■ 
0  0 

0 

0 


0 
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V 


^  '  B91Kl(k) 


Ag^-Bg^Ck) 


V  “N/'l 


bn  Ki(k) 

O  o  A 
'  BK2l(k) 


-Bg^iCk) 


-B01K22(k) 


V-BNQKl(k) 

-B92K22^ 


and  B^  are  the  estimates  of  A  and  B  given  the  information  up  to  N.  . 

o  lNo  N 

—  o  ~ 

It  can  be  easily  discerned  that  the  matrix  A^  closely  resembles  to  A^ 
in  equation  (3.8)  with  the  difference  that  the  estimates  for  A  and  B  are 
used  instead  of  the  exact  matrices.  This  is,  indeed,  one  reason  for  the 
solution  to  be  suboptimal. 

The  above  theorem  provides  a  method  to  determine  a  control  for  c 
N 

fixed  W  .  However,  before  the  outline  for  the  entire  suboptimal  is 

described,  it  will  be  necessary  to  compute  the  cost  (performance  index) 

N  N 

o  ,  o 

associated  with  the  specific  choice  of  W  .  Naturally,  the  specific  W 

N 

o 

which  results  in  the  minimum  cost  will  be  chosen  as  the  optimum  W  ,  and 

N 

o 

the  control  which  is  associated  with  this  W  will  be  the  optimal  control 
signal  at  stage  Nq.  The  derivation  of  the  equations  for  evaluating  the 


cost  can  be  found  in  Appendix  A.  This  cost  is 

No+v-l_ 

JN  =  PN  %  +  Vw  traceii  ’  ^Sn  +lPi)l  ^ 


(3. 


0  0  0 


where  P  satisfies  the  following  propagation  equation 

P.  =  Q.  +  K.'RK,  +  (A.  °(K.)-B.  °K.)  'P.  ,  ,  (A.  °(K.  )-B.  °K.  ) 

X  XX  N  X  x  X  X  X  X"t“X  X  X  X  X 


P„  .  =  Q„  .  i  =  N  ,  N  +1,  .  . .  ,N  +  v-2 
N  +v-l  XN  +v-l  o  o  ’  o 

o  o 


14) 


(3.15) 
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Equipped  with  the  previously  stated  theorem  and  equations  for 
computing  the  performance  index,  a  procedure  for  finding  a  suboptimal 
solution  for  minimizing  J  in  equation  (3.9)  subject  to  equation  (3.8)  is 
outlined  as  follows: 


a) 

b) 

c) 


d) 

e) 

f) 


Compute  the  estimates  and  based  on  the  information 

o  o 

received  up  to  N  . 

r  o 

N 

Choose  a  specific  W  °. 

Apply  the  theorem,  that  is,  use  equations  (3.11),  (3.12),  (3.13) 
to  solve  for  the  sequence  K^,  k = Nq+v, . . . ,Nq  backward  in  time, 
and  compute  u^Q. 

Find  the  corresponding  cost  J  from  equations  (3.14)  and  (3.15). 

No 

Repeat  steps  (b)  through  (d)  for  the  other  finite  number  of 
N 

choices  of  W  °. 

N 

Apply  the  control  u  ,  which  correspond  to  the  v^lue  of  W  0 

o 

which  results  in  the  minimum  cost  J  ,  at  time  N  to  the  system. 

M  N  o 

N  o 

This  yields  the  optimum  W  0  and  u 

N 

o 


3.1.3.  Feedback  Solution  to  the  Magnetic  Ball  System 

In  the  magnetic  ball  suspension  model,  since  there  are  two  states 
and  two  unknown  parameters,  there  are  four  sensitivity  states  and  an  aug¬ 
mented  system  of  sixth  order.  Hence,  the  feedback  gain  K.  is  a  row  matrix 

N  x 

o 

with  six  entries.  With  the  matrix  P.  in  equation  (3.12)  denoted  bv 

N  »N  1 
P.  °=  [Pj  !  (i)  ]  ,  B.°=  [0,b2(i),0,0,0,0]  '  and 
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aL(i)  a2(i) 

J  a3(i)  a4(i) 


where  a^(i)  =  9^,  b?  (i)  =S?  are  Che  unknown  parameters,  and  also  with  the 
feedback  gains  Ki  =  [K^(i)  K^(i)  K92(i)]  denoted  by 

Ki  =  [ki(i)  k?(i)  k^(i)  k,  (i)  k^(i)  kg(i)],  equation  (3.13)  can  be  solved 
sequentially  as: 

a1(i)b2(i)p12(i+l)  +a3(i)b2  (i)p7  (i+l)+b  (i)p  (i+1) 
kl(1)  ’  - = - MW  ~ - - - ~ - 

a9  (i)b2(i)pl2  (i+1)  +  a4(i)b9  (i)p99  (i+1) 

V»  ’  - (¥) - = - “ - 


where 


k3(i) 

k4(i) 

k5(i) 

k6(i) 


aL(i)b2 (i)p32 (i+1)  +  (a3(i)-b2 (i)kL (i) ) (b9 (i)p42 (i+1)) 

(s)(t) 


a2 (i)b, (i)p32 (i+1)  +  (a4(i) -b, (i)k2 (i) ) (b, (i)P42 (i+1)) 

(s)(t) 

aL(i)b9  (i)p52(i+l)  +  (a3(i)-b9  (i)k;[(i)  (b9  (i)p62(i+l)) 

(s)(c) 


a2 (i)b9 (i)p52 (i+1)  + (a4(i)-b2 (i)k9 (i) ) (b, (i)p62 (i+1) ) 

(s)(t) 


(3.16) 


b2(i)P62(i+1) 

S  —  1  4- 

R  +  P22(i+l)b“(i) 
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t 


R  +  p29  (i+l)t>2  (i) 


for  i-N  ,...N  +v-2,  K  ,  ,  =  0. 

°  0  \  +  '1  No 

Hence,  given  a  fixed  W  and  Che  terminal  condition  in  equation 
N 

(3.12),  that  is,  PN°  +  V_L  =  QNo  +  V-1  =<^i  =  $>  where  Q  is  constant  for  the 
system  under  consideration,  equation  (3.13)  can  be  solved  by  cycling 


through  the  sets  of  equations  in  (3.16)  sequentially.  The  resulting  feed- 

N 

~  o 

back  gains  are  then  subsitituted  into  the  matrix  A.  (K.)  in  equation 
N 

o 

(3.12)  and  P.  is  then  obtained.  Cycling  through  equations  (3.12)  and 


(3.13)  will  result  in  K  which  is  then  subsitituted  into  equation  (3.11) 

1  o 

to  obtain  the  control  u  .  A  list  of  the  numerical  values  of  the  matrices 

No 

involved  in  equations  (3.12)  and  (3.13)  are  tabulated  in  Appendix  B.  The 


various  choices  (corners)  of  the  weighting  matrix  W  used  in  the  simulation 

work  are  also  listed  in  Appendix  B. 

As  the  model  studied  in  this  thesis  consists  of  only  a  scalar 

control,  it  is  readily  observed  that  the  matrix  inversion  in  equation 

(3.12)  reduces  to  a  simple  scalar  division.  In  the  general  case  where 

multi-variable  control  is  involved,  one  can  cycle  through  eauations  (3.12) 

~No 

and  (3.13)  by  considering  only  one  column  of  the  B^  matrix  during  each 
cycle.  By  going  through  equations  (3.12)  and  (3.13)  m  times,  where  m  is 
the  number  of  control  variables,  the  m  control  values  can  be  computed 
involving  only  scalar  division,  thus  avoiding  matrix  inversion. 
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3.2.  Factorization  Method 

During  recent  years,  it  has  come  to  be  realized  that  algorithm 
formulations  that  make  use  of  matrix  factorization  generally  enhance 
numerical  stability  [2,3].  Hence,  in  going  through  the  computations  in 
equation  (3.12)  for  the  covariance  update,  covariance  factorization  techni¬ 
que  [3,8]  and  the  rank-one  update  factorization  technique  [2]  is  used.  The 
method  is  described  as  follows: 

N 

O  A  ^ 

1.  Assume  is  given  in  U-D  factor  form.  The  objective 

No  -  -  - 

is  to  obtain  P^  m  U-D  factor  rorm,  where  U,  U  are  unit 

«  — 

upper  triangular  matrices,  and  D,  D  are  diagonal  matrices. 

2.  Apply  the  Modified  Gram-Schmidt  Time  Update  Algorithm 
to  the  following  expression: 

_  N  N  _  N 

UDU '  =  Q.  +  A !  °(K.)P°  A]  °(K.)  (3.17) 

i  i  t  1+1  11 

where  U  is  upper  triangular  and  D  is  diagonal. 

N  N  N  N  N  N 

~o  o~o  ~.oo~o 

3.  Define  v=A.  (K.)P.,.B.  and  a  =  R  +  3  !  P.^B.  , 

i  i  i+l  i  i  i+l  i 

we  then  have 

N  _  , 

P  0  =  UDU'  =  UDU'  -  a  w'  (3.18) 

4.  Apply  rank-one  uDdate  to  equation  (3.18)  to  obtain  the 

N 

—  —  o 

U-D  factor  for  P^  . 

The  factorization  algorithms  used  in  the  previous  procedure  are, 
for  completeness's  sake,  summarized  in  Appendix  C.  The  computer  coding  to 
implement  these  algorithms  are  listed  in  Appendix  D.  It  should  be  noted 
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Chat  the  computer  coding  used  during  the  simulation  work  has  not  made 
full  use  of  the  efficiency  which  is  inherent  in  the  factorization 
techniques,  since  the  efficient  coding  of  the  algorithms  may  result  in 
programs  which  are  unreadable  and  relatively  complicated  to  debug.  None¬ 
theless,  it  should  be  remembered  that  factorization  techniques  do  have 
the  characteristic  of  computational  efficiency  and  the  advantage  of  smaller 
computer  storage  requirement. 


->■? 


CHAPTER  4 
ESTIMATOR  DESIGN 

The  design  of  the  controller  is  based  on  the  assumption  that  the 
estimates  for  the  unknown  parameters  in  the  matrices  A,  B  are  provided  by 
an  estimator.  It  is  the  objective  of  this  chapter  to  develop  an  estimator 
for  the  parameters  of  this  magnetic  ball  suspension  system. 


4.1  Extended  Kalman  Filter 


We  investigate  the  use  of  the  Kalman  filter  [7,11,15]  to  estimate 
the  two  unknown  parameters  0^  and  n  the  system.  We  augment  the  two 
parameters  as  two  additional  states  to  the  system  of  equation  (3.3).  The 
resulting  system  is  an  eighth  order  system 


§  (k+1) 

1 

o 

1 

!(k) 

yk+l  = 

9L(k+l) 

_0_(k+l)_ 

1  1  o 

0  1 

L  !°L 

3x(k) 

92(k) 

n  N  I 

®k  | 

0  i 


uk  + 


(4.1) 


together  with  observation 


where 


Zk=?k+V 


ayk+k 


1  0  0  0  0  0  0 

0  1  0  0  0  0  0 

0  0  1  0  0  0  0 

0  0  0  1  0  0  0 

0  0  0  0  1  0  0 

I  0  0  0  0  0  1  0 


(4.2) 
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and  w^,  are  independent  stochastic  sequences  with  zero  mean,  and  y 
,  "k  are  statistically  independent  Gaussian  variables  with  variances 


sCvJ)  *  V  *£V>j)  ■  “sij-  El>i)  ■  V 


It  should  be  observed  that  the  observation  equation  (4.2)  differs 
from  the  system's  original  observation  equation  (3.1)  by  a  noise  term  TL  . 
This  term  is  appended  to  the  observation  equation  (4.2)  in  order  to  ensure 
that  the  Kalman  filter  to  be  designed  has  non-singular  covariances .  To 
reduce  the  deviation  from  the  original  model  as  much  as  possible,  the 
covariances  of  this  noise  will  be  assumed  to  be  extremely  small.  Specifi¬ 
cally,  the  noise  variance  of  T;  is  taken  to  be  1/100000  times  of  the  V  , 

the  noise  variance  of  w,  . 

k 

With  the  two  unknown  parameters  entering  into  the  system  as  two 
additional  state  variables,  the  new  system  governed  by  equation  (4.1)  is 
a  non-linear  system,  thus  calling  for  the  application  of  an  extended 
Kalman  filter.  The  operation  of  this  extended  Kalman  filter  for  the  magne¬ 
tic  ball  suspension  system  can  be  described  by  the  following  sets  of  equa¬ 
tions  : 


Measurement  Update : 


\+l  =  \  +  Ck(zk-I*k> 

yQ  =  EiyQ}  given 

=  SkH'(HSkH'  +  M)' 


C.  HS. 
k  k 


(4.3) 


(4.4) 


(4.5) 
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where  S  is  given  as  S  =  diag(vn, vn, vn , vn, 0 . 1 , 0 . 11 ,  and  vn  =  y  /10. 
0  O  '  o 

Time  Update : 


k+1 

=  Vyk 

(4.6) 

'k+1 

■  Wi  +  Wi 

(4.7) 

where  2^  is  the  transition  matrix  of  aquation  (4.1)  with  the  feedback 
control  incorporated,  that  is. 


— M  1  -i 

— N  — 

\  !  0 

\  *<*> 

= 

o'10 

+ 

0 

1  0  1 

0 

is  the  linearized  system  transition  matrix,  that  is, 


‘k 


^k 

9yk 


N 


(4.8) 


where  y^  =  (y^(l) ,y^(l) ,y^(2) , . . . ,y^(8) ) '  is  the  nominal  trajectory  for 
the  filter  and  is  obtained  from 


*  N 

Vk 


y 


N 

0 


5(no)  j 

W 


(4.9) 


and  is  the  augmented  matrix  for  the  noise  term  w^ 
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0 


(4.10) 


From  equation  (4.8),  it  is  found  that  all  the  entries  of  the 
matrix  are  the  same  as  the  corresponding  entries  of  the  matrix  3^ 
except  the  following  ones : 


(2.7)  =  y*(l) 

6  N 

(2.8)  =  -iS1ki(j)y^(i) 

^(4,7)  =  yj(3)  +  yj(4) 

^ j  (4,8)  =  -k3(j)yj(3)  -  k4y^(4) 

Yj  (6,7)  =  y^(5)  +  yj(6) 

(6.8)  =  -k5(j)yj(5)  -  k6(J)y^(6) 


where  the  k^'s  are  the  control  feedback  gains  defined  in  equation  (3.16). 

With  the  estimator  and  controller  having  been  developed,  the 
design  of  the  entire  magnetic  ball  suspension  control  system  is  complete. 

A  block  diagram  for  the  discrete  linearized  plant  with  the  SAFER  controller 
and  the  extended  Kalman  filter  is  shown  in  Fig.  3. 
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Fig.  3.  Closed-Loop  Magnetic  Ball  Suspension  System 


4.2.  Filter  Factorization 

As  mentioned  in  Section  3.2  of  this  thesis,  the  factorization 


method  yields  numerically  stable  and  efficient  computations  for  matrix 
algorithms.  The  conventional  Kalman  filter  equations  as  stated  in'  Section 
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4.1  are  particularly  prone  to  numerical  instability  [3,4,8].  In  fact,  it 
is  the  experience  of  the  author  that  during  the  simulation  work  done  on  the 
DEC-10  computer,  computation  underflow  occurred  during  the  matrix  inversion 
for  equation  (4.4).  To  avoid  this  matrix  inversion,  equations  (4.3)  through 
(4.5)  are  cycled  through  once  for  each  of  the  six  components  of  the 
observation  vector  z^.  In  this  manner,  the  matrix  inversion  in  equation 
(4.4)  reduces  to  a  simple  scalar  division.  Nevertheless,  the  underflow 
problem  remained,  probably  due  to  other  matrix  manipulations  in  equations 
(4. 3-4. 5).  It  was  only  after  the  entire  Kalman  filter  was  constructed 
using  factorization  techniques  that  this  problem  of  computation  underflow 
vanished . 

The  Kalman  filter  factorization  algorithm  [2,3]  is  given  in 
Appendix  C  and  the  corresponding  computer  coding  is  given  in  Appendix  D. 

It  should  be  noted  that  in  the  time  update  equation  of  the  estimate  for 
this  magnetic  ball  suspension  system,  a  non-linear  transition  matrix  $  is 
used  and  the  matrix  used  in  the  covariance  propagation  is  the  partial  of 
0^  evaluated  along  the  nominal  trajectory  described  by  the  equation  (4.9). 
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CHAPTER  5 

COMPUTATION  RESULTS 

5.1.  Simulation  of  the  Closed  Loop  System 

Monte  Carlo  type  of  simulation  is  carried  out  on  the  digital 
computer,  with  the  linearized  discrete  system  governed  by  equation  (3.8) 
considered  as  the  plant.  Investigation  into  the  characteristics  of  the 
magnetic  ball  suspension  system  is  done  off  line  through  the  representation 
of  the  system  by  this  plant.  The  sampling  period  for  the  system  is  chosen 
to  be  0.01  second,  which  corresponds  to  100  Hertz.  However,  the  choice  of 
a  sampling  rate  is  rather  artificial  since  the  real  time  linearized  (con¬ 
tinuous)  plant  is  not  implemented.  The  reason  for  studying  the  discrete 
linearized  plant  off  line  only  is  that  the  computation  for  each  of  the 
discrete  control  signals  takes  approximately  seven  seconds  for  the  DEC-10 
computer,  which  makes  the  control  of  the  magnetic  ball  suspension  system  in 
real  time  unfeasible.  The  real  time  magnetic  ball  system  will  be  driven  to 
instability  long  before  the  first  control  signal  is  applied.  A  real  time 
control  algorithm  based  on  this  sensitivity  approach  for  this  particular 
system  will  require  a  faster  computation  device,  more  efficient  implementa¬ 
tion  of  the  algorithm  through  programming  techniques,  and  probably  a  simpli¬ 
fied  model  of  lower  dimension  also.  Hence,  the  simulation  work  done  can 
be  thought  of  primarily  as  an  exercise  to  study  a  certain  stochastic  system 
under  the  application  of  the  sensitivity  approach  control  algorithm,  bearing 
in  mind  that  this  system  bears  close  resemblance  to  the  magnetic  ball  sus¬ 
pension  system  and  it  may  actually  represent  the  magnetic  ball  system  if 
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some  of  Che  hindrance  in  implementations  are  overcome. 

Going  back  to  the  mechanics  of  the  simulation,  only  forty 
iterations  are  carried  out  for  each  set  of  conditions  investigated  as  it  is 
relatively  time  consuming  for  the  computation  of  the  control  at  each  dis¬ 
crete  instant.  With  a  sampling  period  of  0.01  second,  the  forty  iterations 
correspond  to  a  time  span  of  0.4  second.  This  time  interval  should  be 
quite  sufficient  for  avoiding  loss  of  stability  of  the  system,  based  on 
experience  obtained  from  real  time  control  experiments  done  on  the  analog 
version  of  the  magnetic  ball  system  at  the  University  of  Illinois  and  based 
also  on  some  hindsight  after  the  digital  simulation.  A  flow  chart  illustrat¬ 
ing  the  various  steps  involved  in  the  simulation  process  is  given  in  Fig.  4. 
The  plant  under  consideration  has  the  sensitivity  states  incorporated  into 
it  and  is  represented  by  equation  (3,8).  If  the  control  is  to  be  done  in 
real  time,  these  sensitivity  states  will  have  to  be  generated  by  a  separate 
mathematical  model  by  some  processor  while  the  position  and  velocity,  which 
constitutes  the  original  state  variables  of  the  magnetic  ball  system,  will 
be  measured  by  some  transducers. 

3.2.  Stability  of  System 

It  is  well  known  that  for  a  linear  discrete  time  invariant  system, 
asymptotic  stability  of  the  system  is  guaranteed  if  there  exists  a  positive 
definite  solution  to  the  time  invariant  matrix  Riccati  equation  (3.12).  See 
[ 1]  for  example.  Since  solving  for  such  a  solution  may  be  extremely  com¬ 
plicated  and  involved,  an  alternative  which  may  provide  a  general  indication 
of  the  system's  stability  is  sought.  In  this  magnetic  ball  suspension  system, 
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Figure  4  Flow  chart  for  simulation 
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a  rough  idea  concerning  the  stability  of  the  system  is  determined  by  check¬ 
ing  the  eigenvalues  of  the  system's  transition  matrix  with  feedback  incor- 

~N  J* 

porated.  That  is,  the  eigenvalues  of  the  matrix  (A^° (K^)  -  B^°K^)  in  equa¬ 
tion  (3. 15)  are  checked.  if  the  eigenvalues  are  within  the  unit  circle, 
asymptotic  stability  is  supposed  to  be  satisfied.  Indeed,  from  the  experience 
obtained  from  the  simulation  work,  if  the  eigenvalues  are  outside  the  unit 
circle,  the  system  will  diverge.  In  cases  where  the  eigenvalues  are  within 
the  unit  circle,  the  states  do  decay  to  zero.  Hence,  this  method  does  pro¬ 
vide  a  good  approximate  indication  of  the  stability  of  the  system. 


5.2.1.  Effect  of  Weighting  Matrices  on  Stability 


N 

The  choice  of  the  various  weighting  matrices  Q,  R,  \w  0  and  the 
number  of  stages  to  be  optimized  will  influence  the  location  of  the 
eigenvalues  of  the  system  governed  by  equation  (3.8).  Certain  choices  of 
these  weighting  matrices  will  result  in  a  system  with  three  eigenvalues 
located  far  apart  from  the  other  three.  That  is,  there  is  a  set  of  three 
slow  eigenvalues  and  also  another  set  of  fast  eigenvalues  for  the  system. 
This  presence  of  slow  and  fast  time  constants  in  the  system  results  in  an 
unsatisfactory  control  performance.  The  position  of  the  ball,  for  instance, 
will  return  to  the  nominal  position  very  slowly  since  the  velocity  of  the 
ball  approaches  zero  much  faster  than  the  position's  deviation  approaching 
zero.  This  kind  of  situation  is  generally  undesirable  for  such  a  system. 

It  is  found  that  a  choice  of  the  matrix  Q  with  a  large  weight  on  the  posi¬ 
tion  and  a  comparatively  small  weight  on  the  velocity  will  yield  a  system 
with  all  six  eigenvalues  close  by.  Moreover,  a  small  value  for  R  will 
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result  in  fast  eigenvalues  corresponding  to  the  velocity  and  its  sensitivity 

states.  A  large  value  for  X  will  also  result  in  the  same  situation.  It 

should  be  noted  that  a  small  value  for  X  corresponds  to  a  bigger  estimation 

budget,  which  should  enhance  the  accuracy  for  the  estimates.  However,  a  X 

value  which  is  too  small  may  drive  the  system  into  instability.  Along  side 

N 

with  small  X,  a  more  evenly  distributed  element  values  of  the  matrix  W  will 

result  in  faster  eigenvalues  for  the  velocity  and  its  corresponding  sensiti- 

N 

vity  states.  (The  matrix  W  = diag (0. 4, 0. 2 , G.2 , 0 .2 )  is  considered  to  have 

N 

o 

its  weights  more  evenly  distributed  than  W  =  diag (0. 7 , 0. 1 , 0. 1, 0. 1) ) . 

Finally,  increasing  the  number  of  stages  of  optimization  v,  will  enhance  the 
stability  of  the  system. 

Only  after  numerous  experiments  with  the  system,  considering 

both  the  stability  of  the  system  as  well  as  the  accurate  estimation  of 

parameters,  were  the  values  of  the  weighting  matrices  chosen.  The 

values  for  some  of  them  are  listed  in  Table  2.  The  various  choices  of  the 
N 

matrix  W  are  listed  in  Appendix  B. 


Table  2 

Values  for  some  of  the  weighting  matrices 


Q  = 


400  0 

!  0  0.2 


X  =  1 
R  =  0.5 


v  =  5 
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5.3.  Simulation  Results 

The  behavior  of  the  system  was  explored  by  running  the  system 
under  various  conditions.  Noise  was  injected  into  the  system  according  to 
the  system  equation  (3.8)  using  a  programmed  random  noise  generator.  The 
various  sets  of  condition  applied  to  the  system  are  listed  in  Table  3.  All 
the  initial  sensitivity  states  are  assumed  to  be  zero.  For  condition  (15), 
the  actual  parameters  in  the  plant  are  changed  at  t  =  0.2  seconds.  For 
condition  (16),  in  addition  to  the  changing  of  parameters  at  t=0.2  seconds, 
the  position  and  velocity  states  are  also  perturbed.  These  cases  are  used 
to  investigate  the  response  of  the  system  with  time  varying  parameters.  This 
will  be  further  discussed  later  in  the  thesis. 

The  time  response  of  the  states  x^  (position  deviation)  and 
(velocity  deviation)  are  shown  in  Figures  (5 . la-5 . 16a) .  The  error  of  the 

A  A  ~ 

estimates  e]_  =  and  &2=^2~^n’  w^ere  and  ^  are  estitnates  f°r 

9^  and  9  respectively,  are  shown  in  Figures  (5 .  lb-5 . 16b) .  The  control 
response  corresponding  to  conditions  (5),  (6),  (7),  (15),  and  (16)  are  also 
shown  in  Figures  5.5c,  5.6c,  5.7c,  5.15c  and  5.16c  respectively. 


5.3.1.  Region  of  Stability 


In  conditions  (1)  through  (4),  the  initial  position  deviation  is 
set  at  +/-0.05  m. ,  which  is  a  25%  deviation  from  the  nominal  value  of  0.2  m. . 
As  shown  in  Figures  (5. la-5. 4a),  the  states  invariably  decay  to  zero.  Hence, 
a  reasonably  wide  region  of  stability  can  be  inferred  from  these  simulation 


results . 
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Table  3  Initial  conditions  and  parameter's  values  for  simulation 
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Figure  5.4b  Time  response  of  the  estimates'  error 
corresponding  to  condition  (4) 
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Figure  5.5a  State  response  corresponding  to  condition  (5) 


^  1  1  1  1  1  1  1  1  1  j  1  1  1  1  | — 1 — 1  1 — 1 — i 

a. a  a.  1  a. a  a. a  a. 


^  1  1  1  1  j  1  1  1  1  j  1  1  1  1  | — 1 — 1  1 — 1 — j 

a. a  a.  1  a. a  a. a  a. 


Figure  5.5b  Time  response  of  the  estimates'  error 
corresponding  to  condition  (5) 
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Figure  5.6a  State  response  corresponding  to  condit 
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Figure  5.6b 
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Figure  5.7c  Control  response  corresponding  to  condition  (7) 
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Figure  5.6c  Control  response  corresponding  to  condition  (6) 


Figure  5.11a  State  response  corresponding  to  condition  (11) 
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Figure  5.15b  Time  response  of  the  estimates'  error 
corresponding  to  condition  (15) 
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Figure  5.16b  Time  response  of  the  estimates'  error 
corresponding  to  condition  (16) 


Figure  5.15c  Control  response  corresponding  to  condition  (15) 


Figure  5.16c  Control  response  corresponding  to  condition  (16) 
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In  condition  (5),  a  milder  position  deviation,  namely  0.005  m. 
is  used  as  the  initial  condition.  This  yields  a  velocity  response  with 
smaller  magnitude  variation,  which  is  desirable  to  the  system.  Hence, 
through  the  rest  of  the  simulation,  that  is,  for  conditions  (6-14),  this 
particular  initial  position  and  velocity  is  used  so  that  the  resulting 
responses  can  be  compared  accordingly  to  extract  some  information  on  the 
characteristics  of  the  system. 

5.3.2.  The  Role  of  Control  Signal  in  Estimation 

As  observed  from  the  plots  of  the  error  of  the  parameters  for 
conditions  (1-5),  the  estimates  for  39  are  more  accurate  than  those  for 
3^.  This  is,  in  fact,  a  particularly  good  illustration  of  the  feature  of 
this  SAFER  control  algorithm:  the  parameter  which  is  more  crucial  to  the 
control  objective  of  the  system  is  estimated  with  more  effort  and  thus  is 
more  accurately  estimated.  In  this  magnetic  ball  suspension  system,  the 
parameter  9^  is  in  the  A  matrix  in  equation  (2.12);  on  the  other  hand,  the 
other  parameter  appears  in  the  equation's  B  matrix,  which  directly 
governs  the  feedback  gains,  and  thus  also  the  control  signal  to  the  system. 
The  parameter  09  will  directly  affect  the  system's  stability,  which  con¬ 
stitutes  the  control  objective  of  this  system.  Meanwhile,  the  parameter 
3^  does  not  have  the  same  significance  as  99  as  far  as  the  system's  stability 
is  concerned,  thus  its  receiving  a  smaller  estimation  effort  and  not  being 
estimated  as  accurately. 

Another  characteristic  of  this  control  algorithm  chat  can  be 
inferred  from  the  response  curves  of  the  system  is  that  as  the  states,  thus 
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also  the  feedback  control  signal,  approach  zero,  the  parameters  are  not 
estimated  any  more  and  steady  state  errors  are  incurred  for  them.  One 
reason  for  this  situation  is  that  estimation  requires  part  of  the  control 
signal.  As  the  control  signal  approaches  zero,  even  if  the  entire  control 
signal  strength  is  dedicated  to  estimation,  there  may  still  be  insufficient 
effort  for  estimation.  Another  reason  for  this  steady  state  error  of  the 
estimates  is  that  as  the  system's  states  approach  zero,  the  control  objec¬ 
tive  of  this  system  is  essentially  achieved  and  the  need  to  estimate  these 
unknown  parameters  to  fulfill  the  control  objective  is  not  at  issue  any 
more.  Incidentally,  these  points  may  serve  as  excellent  illustrations  to 
the  statement  that  is  made  earlier  in  the  thesis  concerning  the  conflicting 
aspects  of  good  control  and  good  estimation:  a  large  control  signal  may  be 
needed  for  good  estimation  while  a  small  signal  may  be  required  for  good 
control. 

5.3.3.  Effect  of  Process  Noise  Level 

The  noise  variance  of  w^  is  set  originally  at  0.01.  In  condi¬ 
tions  (6)  and  (7),  this  variance  is  changed  to  0.5  and  0.0055  respectively, 
which  represents  a  50  fold  increase  and  0.055  times  decrease  respectively. 
This  should  be  quite  sufficient  for  obtaining  some  information  on  the 
characteristics  of  the  system  under  various  noise  levels.  The  time  res- 
responses  corresponding  to  these  conditions  are  shown  in  Figures  5.6a, 

5.6b,  5.6c,  5.7a,  5.7b  and  5.7c.  Intuitively,  a  smaller  noise  variance 
should  yield  smoother  responses,  which  is  indeed  the  situation  as  shown 
in  the  response  plots  of  Figures  5.7a  and  5.7c  for  the  states  and 
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control  respectively.  On  the  other  hand,  in  Figures  5.6a  and  5.6c,  the 
states  and  control,  which  correspond  to  a  much  larger  noise  variance,  do 
show  more  abrupt  changes.  Nonetheless,  in  both  cases,  stability  of  the 
system  is  achieved  and  the  system  is  capable  of  operating  under  these 
noise  levels  satisfactorily. 

From  the  response  plots  of  the  estimates'  error  in  Figures  5.6b 
and  5.7b,  the  interplay  between  the  control  signal  and  estimation  effort 
is  again  observed.  In  Fig.  5.6b,  the  parameters,  especially  which  in¬ 
fluences  the  control  signal,  exhibit  a  more  oscillatory  behavior,  indicating 
a  tendency  for  the  system  to  estimate  them  as  long  as  the  strength  of  the 
control  signal  is  not  too  weak. 

5.3.4.  Tracking  Ability  of  Filter 

To  ensure  that  the  system  will  operate  in  a  satisfactory  fashion 
even  with  the  unknown  parameters  deviating  quite  a  bit  from  the  expected 
values,  simulation  is  carried  out  to  test  the  tracking  capability  of  the 
estimator.  To  attain  this  goal,  the  values  of  the  parameters  in  the  plant, 
which  are  used  to  generate  the  true  states  or  the  measurement  of  the  magne¬ 
tic  ball  system,  are  varied  individually  as  well  as  simultaneously  by  2 0% 
to  257c.  These  conditions  are  tabulated  in  Table  3  as  conditions  (8) 
through  (13).  In  all  these  cases,  it  can  be  inferred  from  their  corresponding 
state  response  plots  that  as  far  as  stability  is  concerned,  the  system  is 
performing  well  even  though  steady  state  errors  are  incurred  for  the  para¬ 
meter  estimates.  The  parameter  which  directly  affects  the  control  sig¬ 
nal  is  invariably  tracked  to  within  an  error  of  magnitude  0.02,  which  is  a 


5%  error.  On  the  other  hand,  the  estimate  for  9  is  comparatively  worse 
off,  with  an  error  magnitude  which  runs  to  0.13,  which  is  a  267,  error,  in 
the  worst  case.  This  situation  is  probably  due,  again,  to  the  significance 
of  the  parameter  9»  to  the  control  objective  of  the  system,  as  explained 
in  section  5.2.2. 

From  the  response  plots  for  conditions  (8)  through  (13),  it  can 
also  be  concluded  that  the  extended  Kalman  filter  coupled  with  the  SAFER 
Control  Algorithm  is  indeed  tracking  the  parameter  which  is  significant  to 
the  control  objective  of  the  system.  Another  characteristic  concerning 
the  system  that  may  be  inferred  from  these  simulations  is  that  the  system 
is  relatively  insensitive  to  the  parameter  9^,  thus  tolerating  a  larger 
magnitude  of  error  for  it  compared  to  the  other  parameter  of  the  system. 
This  point  can  be  further  illustrated  from  the  response  plots  for  condition 
(14),  Figures  5.14a  and  5.14b.  In  condition  (14),  the  initial  esti¬ 
mate  for  9 ^  is  set  at  the  value  used  in  the  plant.  From  Fig.  5.14b,  it 
is  observed  that  even  with  zero  initial  error,  the  estimate  for  9^  settles 
at  another  value.  However,  as  in  earlier  cases,  stability  of  system  is 
still  achieved  in  spite  of  steady  state  errors  in  the  estimates.  This 
particular  aspect  of  the  system  can  certainly  be  tolerated  if  knowledge  of 
the  exact  values  of  the  unknown  parameters  is  not  strongly  demanded. 

5.3.5.  Effect  of  Time  Varying  Parameters 

In  a  real  world  magnetic  ball  suspension  system,  the  mass  of  the 


ball,  the  bias  current  for  the  control  system,  and  even  the  suspension 
structure  may  change  due  to  environmental  variations  or  the  system’s 


internal  disturbances.  Corrosion,  for  instance,  may  cause  change  in  the 
mass  of  the  ball  and  the  suspension  structure.  Anyway,  these  changes  will 
be  reflected  in  the  variation  of  the  values  for  m,  i  and  the  coil  structure 
constant  c,  which  will  in  turn  result  in  changes  for  the  values  of  the 
parameters  9^  and  9 9 .  To  simulate  these  changes,  the  plant  parameters  are 
changed  at  t=0.2  seconds  in  conditions  (15)  and  (16)  as  tabulated  in 
Table  3.  Furthermore,  in  condition  (16),  in  addition  to  the  changes  of 
plant  parameters,  the  states  (position  and  velocity)  are  perturbed  so  as 
to  add  a  more  realistic  flavor  to  the  simulation.  The  changing  of  the 
states  at  the  instants  when  the  parameters  are  varied  can,  for  instance, 
model  an  increase  in  the  mass  of  the  ball,  which  in  turn  results  in  a 
further  deviation  from  the  nominal  position  and  an  increase  in  velocity 
of  the  ball. 

From  the  response  curves  of  condition  (15)  given  in  Figures  5.15a 
and  5.15b,  it  is  observed  that  at  the  instants  when  the  parameters  are 
changed,  the  states  have  almost  decayed  to  zero.  Since  the  states  have  not 
been  affected  by  the  change,  this  together  with  the  weakness  of  the  feed¬ 
back  control  signal  results  in  a  situation  in  which  the  system  makes  no 
attempt  to  estimate  the  new  parameter  values  and  simply  tolerates  the 
large  steady  state  errors  for  the  parameters.  Reemphasizing  the  charac¬ 
teristic  of  this  closed-loop  system  mentioned  earlier:  as  the  stability 
of  the  system  is  attained,  the  estimation  of  the  parameters  are  not  at 
issue  any  more. 

In  the  simulation  of  condition  (16),  the  perturbation  of  the 
states  results  in  a  stronger  feedback  control  signal  which  will  in  turn 
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allow  for  a  larger  estimation  effort.  Furthermore,  since  the  system's 
stability  is  disturbed  during  this  change  of  the  parameters,  more  effort 
is  put  into  the  estimation  task  to  estimate  the  parameter  which  is 
crucial  to  the  fulfillment  of  the  system's  objective.  Indeed,  from  the 
plot  of  the  error  of  the  estimates  in  Fig.  5.16b,  it  is  observed  that  after 
the  parameters  are  changed  and  the  states  perturbed,  the  filter  exhibits 
an  effort  to  track  the  parameters.  However,  the  estimates  finally  settle 
with  some  steady  state  error  as  stability  of  the  system  is  regained  and 
accurate  estimation  of  the  parameters  is  no  longer  necessary. 
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CHAPTER  6 
CONCLUSION 

la  this  thesis,  application  of  the  Sensitivity  Adaptive  Feedback 
with  Estimation  Redistribution  (SAFER)  control  algorithm  to  the  magnetic 
ball  suspension  system  is  considered.  This  particular  studv  serves  as  an 
example  where  an  adaptive  control  algorithm  is  applied  to  a  stochastic 
system  with  unknown  parameters. 

From  the  simulation  results,  it  can  be  concluded  that  the  magnetic 
ball  suspension  system  can  indeed  operate  satisfactorily  under  various 
levels  of  noise  disturbances  and  rather  large  deviation  from  nominal  posi¬ 
tion  under  the  application  of  a  sensitivity  approach  adaptive  control 
algorithm.  It  should  be  noted  that  the  criterion  for  performance  of  the 
system  is  primarily  its  stability  rather  than  the  accurate  estimation  of 
the  unknown  parameters  in  the  system.  The  feature  of  this  sensitivity 
approach  control  algorithm  is  its  capability  of  distributing  more  estima¬ 
tion  effort  for  estimation  of  the  parameters  which  is  more  significant  to 
the  control  objective  of  the  system.  This  is  well  exhibited  in  the  simula¬ 
tion  work  done  in  this  thesis. 

Factorization  techniques  are  applied  to  various  matrix  manipulation 
algorithms  which  are  needed  in  the  controller  and  estimator  design.  The 
utilization  of  these  factorization  algorithms  has  enhanced  numerical  stability 
and  in  particular,  eliminated  numerical  underflow  problems  for  the  computa¬ 
tions  in  the  extended  Kalman  filter  designed. 

Meanwhile,  there  does  exist  hindrance  to  the  application  of  this 
sophisticated  and  rather  complex  control  algorithm  to  control  systems  in 
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real  time.  This  control  algorithm  may  be  more  applicable  to  processes 
which  advance  relatively  slow  in  time,  certain  chemical  processes,  for 
instance.  Alternatives  to  overcome  the  lengthy  computational  time  may  in¬ 
clude  further  simplification  of  the  control  algorithm,  or  reducing  the 
dimensionality  of  the  model  as  well  as  search  for  more  efficient  computational 
methods  and  devices. 
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APPENDIX  A 


DERIVATION  OF  THE  COST  EQUATIONS 


This  appendix  is  concerned  with  the  derivation  of  the  cost 
equations  (3.14)  and  (3.15).  The  objective  is  to  find  the  cost  J  given 
by  equation  (3.9).  Identical  results  can  be  found  in  reference  [12]. 


Given  the  cost  J 


N  +v-i 
o 


J  Elk=N  5k\§k+UkRuk^ 


(A.l) 


Assume  the  cost  at  Nq+ v,  JN  +v~0,  then  the  cost  to  go  at  N  +v-l  can  be 

o  0 

written  as : 


where 


JN  +  v-i  E*-s  N  +v-1Pn  +v-15N  +v-lj  +0:N  +v-1 
°  °  0  0  o 


(A. 2) 


P  =0 

N  +v-l  VN  +v-l 
o  o 


X+v-l  ^N  +v  +  E *"  ^^N  +v-l)  Rn  +v-1^n  +v-l')'f  (A. 3) 


g-  =0 
N  +v 
o 


Continuing  in  this  fashion,  the  cost  to  go  at  stage  k  can  be  written  as: 

N  +v-i 
o 

Jk  =  ElJk  ^(Q.+K-RK.)!.] 


Ei?k«k  +  WV+Jk+l 


(A.  4) 
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Assuming  the  cost  to  go  J,  to  be  of  the  form: 


Jk  =  E^kPkM  +  ak 


Substitute  equation  (A. 5)  into  equation  (A. 4) 


(A.  5) 


Jk  ■  ^Wk  +  ’W^  +  E^k+lPk+l!kU]+“k+l 


(A.  6) 


N  N 

—  Q  *■»  O  ~ 

Using  the  system  equation  (3.8)  with  the  matrices  A^  ,  B^  replacing  A^, 
to  obtain  in  equation  (A. 6),  we  have: 


Jk  ■  El!i«k  +  WU 

-  Et5i<\°<Kk> vAX>  -“kVv 


+  El<F»  >'Pk+l(rw  >i  +  °Wl 
k  k 


or 


Jk  ■  Et5A  +  + 


+  (vu)tracelr'Pk+lri  +  Vl 


Comparing  (A. 5)  with  (A. 7),  we  have  the  following: 

\  *  \+iWi+ <\°<v  -  £vwv«ic>  ■  »Iv 


PN  +v-l  ~  +v-l  k  =  No’* 
o  o 


a  a  a  ,  , 
k  k+1  w 


N  +v-l 
o 


V  trace(r'P,  ,  T] 
w  k+1  J 


=  0 


k  =  N  , 
o 


,N  +v-2 
*  o 


,N  +v-2 
’  o 


(A.  7) 


(A.  8) 
(A. 9) 
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Combining  equations  (A. 4),  (A. 7),  (A. 8)  and  (A. 9).  the  cost  at  N  can  be 

o 


written  as : 


N  +^-1 
o 


JN  "  PN  +  i=N  +1  Craceir'pirJ 


O  0  0  0 


N  +v-l 
o 


SN  PN  +  (vw)crace^  '  (j_tN 


0  0  0 


A  final  note  is  that  this  optimal  cost  J  can  also  be  expressed  as* 

N  r 


N  +v-l 
o 


JN  =  >N  PN  h  +  (VtraceUi^  +1pi)rr'i 


o  o  o  o 


since  traceiABj  =  tracelBAj  assuming  A  and  B  are  matrices  of  conformable 


dimension. 
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APPENDIX  B 


LIST  OF  VALUES  FOR  MATRICES 
IN  EQUATIONS  (3.12)  AND  (3.13) 


The  values  of  Che  matrices  in  equations  (3.12)  and  (3.13)  are: 


1  0.01 

0 

= 

\  = 

A 

vv  1  _ 

0 

_^2  (^ 

_ 

0 

0 

B,  = 

0 

1 

0 

0 

—  — i 

= 

0 

0 

B  = 

0 

0 

0 

92 

_1 

Q 

0 

400 

0 

N 

Q  - 

0 

\w  0 

_  o 

0.2 

R  =  0.5 
\  =  1 

Q  - 


N 

Corners  of  the  weighting  matrix  W  0  are: 

N 

(i)  W°  =  diag  (0.7,0. 1,0. 1,0.1) 
N 

(ii)  W  0  =  diag  (0.1, 0.7,0. I, 0.1) 
N 

(iii)  W°  =  diag  (0. 1,0. 1,0. 7,0.  L) 
N 

(iv)  W°  =  diag  (0.1, 0.1, 0.1, 0.7) 


I 
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APPENDIX  C 

FILTER  FACTORIZATION  ALGORITHM  AND 
RANK -ONE  UPDATE  ALGORITHM 


Prob lem  Formulation  for  Filter  Factorization. 


The  filter  factorization  and  rank-one  update  algorithms,  which 
can  be  found  in  references  [2,3],  are  summarized  below: 

Given  the  linear  dynamic  system 


x 


i+I 


0.x.  +i.u.  +  G . w. 

i  i  ii  ii 


(C.l) 


and  observation 


2.  =  H.x.  +  v. 
i  ii  i 


(C .  2 ) 


where  x.€Rn,  z.£Rm,  w.,  v.  are  zero  mean  and  x  ,  w.,  v.  are  statistically 
1111  o  i  l  J 

independent  Gaussian  variables  with  covariances 


T  T 

e[w.w.]  =Q.6..,  E[v. v . }  =  R. 6 .  . , 
i  j  l  ij  i  J  i  ij 


E lx  x  ]  =  P 
o  o  o 


The  vector  x^  represents  the  minimum  variance  estimate  of  x^  given  zq, 
z^,...,2^  and  may  be  obtained  from  the  following  Kalman  filter  algorithm. 


Measurement  update: 


x.  =x.  +  K.  (z  .  -  H.  x.  ) 

i  i  i  i  ii 

(C.3) 

-  T  -  T  -1 

K.  =  P.HT(H.P.H.  +  R.) 

i  ii  iii  i 

(C.4) 

P.  =  P.  -  K.H.P. 

i  i  ill 

(C.5) 
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Time  update: 


x.  .  =  0.x.  +  r.u. 

i+i  ii  ii 


P  ,  =  +  g.q.gt 

1+1  111  11  1 


where  u.  is  the  optimal  control  value. 

i 


Initial  conditions 


(C.6) 
(C .  7) 


Xo  =  EUo-*  SiV6n  (C.8) 

Pq  given  (C. 9) 

Suppose  the  n-dimensional  error  covariance  matrix  P,  is 
factored  such  that 

P  =  UDUT  (C.10) 


where  U  is  upper  triangular  with  unit  diagonals  and  D  = diag(d^, . . . ,d  ). 
The  matrices  U  and  D  are  referred  to  as  the  U-D  factors  of  P»  They  are 
unique,  provided  that  P  is  positive  definite  and  can  be  constructed  using 
a  Cholesky  factorization  [2] . 

Algorithms  are  presented  for  performing  measurement  and  time 
updating  of  the  U-D  factors.  These  algorithms  correspond  to  the  conven¬ 
tional  Kalman  formulas  of  equations  (C.4),  (C.5)  and  (C.7). 
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U-D  Measurement  Update  Algorithm 


Given  a  priori  covariance  factors  U  and  D  and  a  scalar  measurement 

2-i 

z  =  Hx  +  v,  where  Eiv  j  =  r,  the  updated  U-D  covariance  factors  and  the 
Kalman  gain  (U,  D  and  K  respectively)  can  be  obtained  as  follows; 


fT  =  Hu  fT  =  . fn) 


(C.ll) 


v  =  Df  v .  =  d . f . 

J  J  J 


(C. 12) 


=  (vi;0,...,0) 


(C. 13) 


a  =  r  +  v  f 
1  11 


(C. 14) 


If  2^=0,  omit  equation  (C.15) 


dL  =  (r/<»1)d1 


(C.15) 


For  j=2,...,n  cycle  through  equations  (C.16-C.20) 


a.  =  a.  ,  +  v.f. 
J  J-l  J  J 


(C. 16) 


If  a  = 0,  omit  equations  (C.17-C.20) 


d.  =  (Qf.  ./Of.) d. 
J  J-l  J  J 


(C. 17) 


If  d .  = 0,  skip  to  equation  (C.20) 


X..  =  f./Of,  , 
J  J  J-l 


(C. 18) 


U.  =  U.  -  \.K.  . 
J  J  J  J-l 


(C. 19) 


K.  =  K.  .  +  v.U . 
J  J-l  J  J 


(C.20) 
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where  U  =  [U^ , • . . ,Un] .  The  component  U  vectors  have  the  form 

Uj  =  [U  (l),...,Uj(j-l),l,0,...,0] 

and  D=diag(d^, _ >dn)*  The  Kalman  gain  is  given  by 

K  =  K  /<*  (C.21) 

n  n 

where  &  is  the  innovation  covariance, 
n 


Modified  Gram-Schmidt  Time  Update  Algorithm 

Modified  Gram-Schmidt  techniques  may  be  used  to  accomplish  time 
updating  of  the  U-D  factors,  and  the  resulting  algorithm  is  the  following. 


Let 


n  P 

W  =  [  G  ]  5  ! 


1 

,T 


w 

n 


n  P 


D  =  diag(D,  Q)  =  diagCd^d^  . . .  ,d  ) 

~  ~  —  —  T 

The  U-D  factors  of  P=WDW  in  equation  (C.7)  may  be  computed  as  follows: 
For  j  = n,n- 1 , . . . , 1  evaluate  equations  (C.22-C.24) 


~  T- 

d  .  =  w .Dw 
J  J  J 


(C.22) 


If  d^  =0,  omit  equations  (C.23-C.24) 
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1  r  T-  . 
u,  .  =  »-  w.Dw, 

lj  dj  i  j- 


w.  :  =  w .  -  u .  w 
i  i  ij  j 


where  Che  symbol  "  denoCes  replacemenC  in  computer  storage. 


(C.23) 

(C.24) 


Rank -One  Update  Algorithm 

Let 

—  - T  T  T 

P  =  UDU  =  UDU  +  caa 


where  c  is  a  scalar,  a  is  an  n  vector,  U  is  unit  upper  triangular, 

D  =  diag (d^ , . . . , d  )  and  n  = dim  P. 

If  P  is  positive  definite,  then  the  factors  U  and  D  can  be 
calculated  as  follows: 

For  j  = n,n-l, . . . ,2  recursively  evaluate  ordered  equations  (C.25)  through 
(C . 28) : 


-  2 

d .  =  d .  +  c .a .  (cn  =  c) 

j  j  j  j  ' 


a>  •  -  au  “  a  (k,  j  ) 


U (k., j )  =  U(k,j)  +  c.a  a  /d. 

J  J  K  J 


>  k  =  l,...,j-l 


c 


J-l 


c.d  /d. 
J  J  J 


(C.25) 

(C.26) 

(C.27) 


and  then  compute 


dl  +  °iai 


(C . 28) 
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APPENDIX  D 


LISTING  OF  COMPUTER  PROGRAMS 


C  TO  EXECUTE  THIS  PROGRAM  IN  DEC-10  COMPUTER: 
C 

C  (1)  ASSIGN  DSK :  2  3EF0RE  EXECUTION 
C  (2)  TO  EXECUTE,  TYPE  THE  FOLLOWING 
C  EX  FILENAME. EXTENSION,  SYS : SSP 1 O/SEARCH 

C 

C  SUBROUTINES  FOR  MATRIX  MANIPULATIONS 
C  THE  MAXIMUM  DIMENSION  OF  MATRIX  IS  (20,20) 

C 

c 

c 

c 

C  SCALAR  MULT.  OF  MATRIX 
C  A  IS  OF  DIMENSION  I *J ,  S  IS  THE  SCALAR 
SUBROUTINE  M ATSM ( A , S , I , J ) 

REAL  A ( I , J ) 

DO  20  M  =  1 , I 
DO  20  N  =  1  ,  J 

20  A(M,N)=A(M,N)*S 

RETURN 
END 
C 
C 
C 

C  MATRIX  MULTIPLICATION 
C  A  IS  I*J,  B  IS  J*:<,  RESULT  IS  C  (I*K) 
SUBROUTINE  M ATMUL ( A , 3  ,  C , I , J , K ) 

REAL  A ( I  ,  J  )  ,  B(J,K),  C(I,K) 

REAL  E (20 , 20) 

DO  20  Msl , I 
DO  20  M  =  1  ,  K 
E ( M , N  )  =0 
DO  20  Lai , J 

E(M,N)=E(M,N)+A(M,L)*B(L,N) 

DO  30  Mai ,  I 
DO  30  N  a  1  ,  K 
C(M,N)=E(M,N) 

RETURN 
END 


20 

30 


1 
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C 

C 

C 

C  MATRIX  TRANSPOSE 
C  A  IS  INPUT  MATRIX  OF  I*J 

SUBROUTINE  M ATTNS ( A , AT , I , J  ) 

REAL  A ( I , J  )  ,  AT  ( J  , I) 

REAL  AE(20, 20) 

DO  20  M  =  1  ,  J 
DO  20  N  =  1  ,  I 

20  AE (M , N ) =A(N , M) 

DO  30  M  =  1  ,  J 
DO  30  N  =  1  ,  I 

30  AT ( M , N ) = AE ( M , N ) 

RETURN 

END 

C 

C 

c 

C  MATRIX  ADDITION (1 ) /SUBTRACTION (2) 

C  A,  B  ARE  OF  DIMENSION  I*J,  C  IS  RESULT 
C  L  IS  1  FOR  ADDITION,  2  FOR  SUBRACTION  (A-3) 
SUBROUTINE  M ATAS ( A , B , C , I , J , L ) 

REAL  A ( I  ,  J  )  ,  B ( I , J )  ,  C ( I  ,  J  ) 

IFCL.EQ.2)  GOTO  40 
DO  20  M  =  1  ,  I 
DO  20  N  =  1  ,  J 

20  C(M,N)=A(M,N)+3(M,N) 

RETURN 

40  DO  50  M  =  1  ,  I 

DO  50  N  =  1  ,  J 

50  C(M,N)=A(M,N)-3(M,N) 

RETURN 

END 


C 

C  CALCULATE  TRACE  OF  MATRIX 
C  A  IS  OF  DIMENSION  L*L ,  S  IS  RESULT 
SUBROUTINE  TRACE ( A , S , L ) 

REAL  A (L , L) 

S=0 

DO  20  N  =  1  ,  L 
20  S  =S+A ( N , N ) 

RETURN 

END 


C 

C  CALCULATE  EUCLIDEAN  NORM  OF  A  VECTOR 
C  A  IS  N  *  1  VECTOR,  XN  IS  RESULTING  NORM  VALUE 
SUBROUTINE  NORM ( A , N , XN  ) 

DIMENSION  A ( N  ,  1  ) 

XN  =0 

DO  10  1=1 ,N 


OOOOOOOOOOOOOOOO  — >  OOOOO  — *  ooooo 
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10  XN=XN+A( I , 1 ) *A( I , 1 ) 

XN=SQRT(XN) 

RETURN 

END 


SUBROUTINE  FOR  INITIALIZING  AN  IDENTITY  MATRIX 
A  IS  TO  SET  TO  IDENTITY  MATRIX  OF  N*N 
SUBROUTINE  MATID ( A , N ) 

REAL  A ( N , N ) 

CALL  MATSM ( A , 0 .  ,N,N) 

DO  10  1=1 , N 
0  A  ( I  ,  I )  =  1  . 

RETURN 

END 


SUBROUTINE  FOR  INITIALIZING  AN  IDENTITY  MATRIX 
A  IS  TO  SET  TO  IDENTITY  MATRIX  OF  N*N 
SUBROUTINE  M AT I D ( A , N ) 

REAL  A ( N , N ) 

CALL  MATSM ( A , 0 • ,  N,N) 

DO  10  1=1 , N 
0  A  ( I  ,  I )  =  1  . 

RETURN 

END 


THIS  SUBROUTINE  GENERATES  A  SEQUENCE  OF  INDEPENDENT 
GAUSSIAN  RANDOM  NUMBERS  WITH  'MEAN'  AND  'VAR',  WH 
CALLED  SUCCESSIVELY.  'MEAN'  AND  'VAR'  CAN  BE  VARI 
EACH  TIME  THE  SUBROUTINE  IS  CALLED. 

THE  FIRST  TIME  THE  SUBROUTINE  IS  CALLED  WITHIN  A  MAIN 
PROGRAM  THE  FIRST  ARGUMENT  'I'  SHOULD  BE  EQUAL  TO 
UNITY. 

THE  MAIN  PROGRAM  SHOULD  BE  EXECUTED 
CALLING  SYS : SSP 10/SEARCH 

SUBROUTINE  RANDOM ( I , VAR , MEAN , RV , KKK ) 

REAL  MEAN 

IF ( I . EQ. 1 )  KKK= 1 

CALL  SETRAN (KKK) 

Y=RAN ( 1 ) 

CALL  NDTRI(Y,X,Z,IER) 

RV=MEAN+X*SQRT(VAR) 

CALL  SAVRAN(XKK) 

RETURN 

END 


lO  Id 
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C 

c 

C 

c 

C  SUBROUTINE  FOR  WRITING  MATRIX  ON  DISK  FOR  LAS  ANALYSI 

n 

Vs 

C  A  IS  M*N  MATRIX 

C  MATRIX  NAMES  SHOULD  BE  GENERATED  IN  THE  FILE 
C  NAMES . DAT  SUCH  THAT  EACH  MATRIX  RECORD  WOULD 
C  CORRESPOND  TO  A  PARTICULAR  NAME 

r 

Vs 

C  ALSO,  PERFORM  "ASSIGN  DSK:2"  BEFORE  MAIN  PROGRAM 
C  EXECUTION 
C 

SUBROUTINE  WDSK ( A , M , N , NR  EC ) 

DIMENSION  A ( M , N ) 

DIMENSION  R ECOR ( 1 03 ) 

CALL  DEFINE  F ILE  ( 2  ,  1  03 , N V ,  ' M ATR IC . DAT  ’  ,  0 , 0 ) 
RECOR ( 1 ) =M*N 
RECOR (2)=M 
RECOR ( 3  )  =N 


NCON  =0 
DO  10  1  =  1  ,  N 
DO  20  J  =  1  ,  M 

20  REC0R(3+J+NC0N)=A( J , I) 

10  NCON  =  1 *M 

WRITE (2  NREC ) RECOR 
TYPE  111,  NREC 

111  FORM AT ( / ’  MATRIX  HAS  RECORD  =  ',16) 

RETURN 


END 

C 

C 

n 

Vs 

c 

c 

c 

C  SUBROUTINES  FOR  GENERATING  NEW  STATES  WITH 
C  LINEARIZED  DISCRETE  PLANT  OF  EQN .  (3-3) 

r 

Vs 


C 

c 


C  THIS  SUBROUTINE  INITIALIZE  THE  SYSTEM  EQUATION 
C  A  MATRIX  WITH  PLANT  PARAMETER  VALUES 
C 


SUBROUTINE  SETRA(A) 

COMMON  A1,A2,A3,A4,32,RA3,RB2 
REAL  A ( 6 , 6 ) 

DO  10  M  =  1 , 6 
DO  10  N  =  1  ,  6 
A ( M , N ) =0 
A  (  1  ,  1  )  =  A  1 
A (  1  , 2)=A2 


10 


ooooooooooo  I\)  oooo  — »  ooooo 
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A  (  2  ,  1  )  =  R  A  3 
A  (  2 , 2 )  =  A  4 
A ( 3 , 3 ) =A  1 
A ( 3 , 4)=A2 
A ( 4 , 1  )  =  1 
A( 4 , 3) =RA3 
A ( 4 , 4 )  =  A  4 
A  (  5 , 5 ) =A  1 
A  ( 5 , 6 )  =A2 
A ( 6 , 5 ) =R A3 
A ( 6 , 6 )  =A4 
RETURN 
END 


CALCULATE  SYSTEM  MATRIX  GIVEN  FEEDBACK  GAINS 

SUBROUTINE  CALRA( A , K , IN ) 

COMMON  A1 ,A2,A3,A4,B2, RA3, RB2 
REAL  A ( 6 , 6 ) ,  K ( 6 , 20 ) 

A ( 4 , 3)=RA3-RB2*K(1 , IN) 

A (4, 4)=A4-RB2*K(2, IN) 

DO  10  N  =  1 , 4 
A(6,N)=-K(N , IN) 

A ( 6 , 5 )  =  R A  3-R32*K ( 1 , IN)-K(5, IN) 
A(6,6)=A4-RB2*K(2,IN)-K(6,IN) 

RETURN 

END 


CALCULATE  MATRIX  FOR  COVARIANCE  PROPAGATION 
SUBROUTINE  C ALRB ( A , K , IN ) 

COMMON  A1,A2,A3,A4,B2,RA3,RB2 
REAL  A ( 6 , 6 ) ,K(6, 20) 

A ( 2 , 1 )=RA3-R32*K( 1 , IN) 
A(2,2)=A4-RB2*K(2,IN) 

DO  20  N=3 , 6 
A(2,N)=-RB2*K(N,IN) 

RETURN 

END 


THE  FOLLOWING  ARE  SUBROUTINES  FOR  THE 
MAGNETIC  BALL  SUSPENSION  MODEL 

THE  ESTIMATES  FOR  THE  PARAMETERS  ARE 
USED  THROUGHOUT  THESE  COMPUTATIONS 


SUBROUTINE  TO  INITIALIZE  SYSTEM  MATRIX  A 
SUBROUTINE  SETA ( A ) 

COMMON  A1 , A 2 , A 3 , A 4 , B2, RA3, RB2 


oooooooo  ro  oooo  -■>  oooo 
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DIMENSION  A ( 6 , 6 ) 
DO  10  M  =  1  ,  6 
DO  10  N  =  1 , 6 
10  A  (  M  ,  N  )  =  0 

A  (  1  ,  1  )  =  A  1 
A  (  1  ,  2 )  =  A  2 
A  (  2  ,  1  )  =  A  3 
A  (  2 , 2 )  =  A  4 
A(3, 3)  =  A  1 
A( 3 ,  4)=A2 
A  (  4  ,  1  )  =  1 
A  (  4  ,  3 )  =  A  3 
A  (  4 , 4  )  =  A  4 
A  ( 5 , 5 )  =  A  1 
A( 5 , 6 ) =A2 
A( 6 , 5 ) =A3 
A( 6 , 6 ) =A4 
RETURN 
END 


CALCULATE  SYSTEM  MATRIX  GIVEN  FEEDBACK  GAINS 
SUBROUTINE  CALA ( A , K , IN ) 

COMMON  A  1 , A2, A3, A4 , 32, RA3, RB2 
REAL  ACS, 5) ,  K ( 6 , 20 ) 

A ( 4 , 3 ) =A  3-B2*K ( 1 , IN) 

A (4, 4 ) = A  4 -B  2  *K ( 2 , IN) 

DO  10  N  =  1 , 4 

0  A(5 , N)=-K(N , IN) 

A(6, 5 ) =A3-B2*K ( 1 , IN ) — K ( 5 , IN) 

A  C  6 , S ) =A4~32*K (2 , IN ) -K (5 , IN) 

RETURN 

END 


CALCULATE  MATRIX  FOR  COVARIANCE  PROPAGATION 
SUBROUTINE  CALBC  A , K , IN) 

COMMON  A1 ,A2,A3,A4,B2,RA3, RB2 
REAL  A  C  6 , 6 ) ,KC6, 20) 

AC  2 , 1 )=A3-B2»K(1 , IN) 

A ( 2 , 2 ) = A 4-32*K  C 2 , IN) 

DO  20  N  =  3 , 6 

0  AC2, N)=-B2*X(N , IN) 

RETURN 

END 


SUBROUTINE  TO  CHANGE  WEIGHTING  MATRIX  W 
VARIOUS  CORNERS  OF  THE  MATRIX  W  IS 
OBTAINED  BY  SPECIFYING  THE  VALUE 
OF  THE  INTEGER  N  ( 1 , 2, 3, 4)  . 


OOOOOOOOOOOOO 
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C 

c 

SUBROUTINE  SWQD (QD , WNO , N , AL ) 
REAL  QD (5,6) 

RWNOsO  .  -WNO ) * AL/3 • 

IF  (N. NE.  1  )  GOTO  20 
QD ( 3 , 3)=WNO*AL 
DO  10  1=4,6 

10  QD ( I , I)=RWNO 

RETURN 

20  IF  (N.NE.2)  GO  TO  40 

QD ( 4 , 4 ) =WNO* AL 
QD ( 3 , 3)=RWN0 
QD (5 , 5 ) =RWNO 
QD (6 , 6 )=RWNO 
RETURN 

40  IF  ( N . NE  .  3 )  GO  TO  60 

QD ( 5 , 5 ) =WNO*AL 
QD ( 3 , 3 ) =RWNO 
QD ( 4 , 4 ) =RWNO 
QD ( 6 , 6 ) =RWNO 
RETURN 

60  QD (6 , 6 ) =WNO*AL 

DO  70  1=3,5 

70  QD ( I , I ) =RWNO 

RETURN 
END 


SUBROUTINE  TO  CALCULATE  COST 
Z  IS  THE  STATE  VALUES  AT  TIME  NO, 

A  IS  THE  SYSTEM  MATRIX,  K  IS  THE  SEQUENCE  OF  FEEDBACK 
GAINS,  QD  IS  THE  AUGMENTED  PENALTY  MATRIX  FOR  THE 
STATES,  GA  IS  THE  AUGMENTED  NOISE  MATRIX,  VW  IS 
THE  VARIANCE  OF  THE  NOISE,  MU  IS  THE  NUMBER  OF 
OPTIMIZATION  STAGE,  AND  COST  IS  THE  RESULT. 


SUBROUTINE  COM P J ( Z , A , K , QD , GA , VW , COST , MU ) 

REAL  A ( 6 , 6  )  ,  P  C  6 , 6  )  ,  F(6,6),  C(6,6),  QD  C  6 , 6  ) 
REAL  Z  (  6  ,  1 )  ,  D  ( 6  ,  1 )  ,  E  ( 1  ,  6 )  ,  AGO, 6),  GA  ( 6  ,  1 ) 
REAL  K ( 6 ,MU) 

DO  20  1=1,6 
DO  20  J=1 , 6 
F  ( I  ,  J  )  =  0 

20  P(I,J)=QD(I,J) 

DO  1 1  M=MU-1 ,1,-1 
CALL  MATAS ( P , F , F , 6 , 6 ,  1 ) 

CALL  SETA ( A ) 

MTsM 

CALL  C ALA ( A , K , MT ) 

CALL  C ALB ( A , K , MT ) 

CALL  MATMUL (P , A , P , 6 , 6 , 6  ) 


oo  oooooooooooo 
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CALL  M ATTNS ( A , C , 6 , 6 ) 

CALL  MATMUL(C,F,P,6,6,6) 

CALL  MATAS (QD , P , P , 6 , 5 , 1 ) 

DO  10  LL  =  1 , 6 

10  E ( 1 , LL ) =K (LL , M) 

CALL  MATTNS (  E  ,  D  , 1 ,6) 

CALL  MATMUL ( D , E , C , 6 , 1 ,6) 

CALL  MATAS ( P , C , P , 6 , 6 , 1 ) 

11  CONTINUE 

CALL  MATTNS (GA , AG , 6 , 1 ) 

CALL  MATMUL ( AG , F , E , 1,6,6) 
CALL  MATMUL (E , GA , COST ,1,6,1) 
COST  =COST*VW 
CALL  MATTNS ( Z , E , 6 , 1) 

CALL  MATMUL (P , Z , D , 6 , 6 , 1 ) 

CALL  MATMUL (E , D , TT , 1,6,1) 

COST  =COST+TT 

RETURN 

END 


SUBROUTINE  FOR  COMPUTING  FEEDBACK  GAINS 
USING  FACTORIZATION  METHODS 
AND  RANK  ONE  UPDATE  ALGORITHM 


R  IS  THE  WEIGHT  FOR  THE  CONTROL  IN  THE  COST  EQUATION. 


SUBROUTINE  COMKUD ( A , B , QD , K , R , MU ) 

REAL  A ( 6 , 6 ) ,  B(6,1),  P(6,6),  K(6,MU) 

REAL  AT ( 6 , 6  )  ,  QD ( 6 , 6 ) 

REAL  V ( 6 , 1 ) ,  U ( 6 , 6 ) ,  UT ( 6 , 6 )  ,  D ( 6 , 6 )  ,  DT ( 6 , 6 ) 
REAL  W(6,12),  DB(12,12),  WP(6,6),  RWD ( 1 , 12) 
REAL  TRWD(12, 1 ) 

COMMON  A1,A2,A3,A4,B2,RA3,RB2 

TO  DETECT  COMPUTATION  OVERFLOW  OR  UNDERFLOW 
CALL  ERRSET (100) 

C 

C 

CALL  MATSM ( W , 0 . ,6,12) 

CALL  MATSM ( DB , 0 . , 12, 12) 

CALL  MATSM ( DT  ,  0 .  ,6,6) 

n 

DO  10  1=1,6 
W(I , 1  +  6 )  =  1  . 

DBCI+6, 1  +  6  )  =QD ( I , I ) 

D ( I , I ) sQD ( I , I ) 

10  CONTINUE 

CALL  MATID(U , 6 ) 

C 
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C 

C  TO  COMPUTE  THE  FEEDBACK  CONTROL  GAINS  SEQUENCE 
DO  160  IN=MU-1 ,1,-1 
C 

CALL  M ATTNS (U , UT , 6 , 6  ) 

CALL  MATMUL (D,UT,P,6,6,6) 

CALL  MATMUL (U,P,P,6,6,6) 

SCA=R+P(2,2)*B2*B2 

SCB=SCA+B2*P(6,2) 

SC A  =  - 1 . / SC  A 

K ( 1 , IN )  =  ( A 1 *B2*P ( 1 , 2)+A3*B2*P(2,2)+32*P(4,2)  )/SCB 
K ( 2 , IN )  =  ( A2*  B2*P ( 1 , 2)+A4*B2*P (2 , 2) ) /SCB 
K(3, IN)  =  (A1*B2»P(3,2)  +  (A3-B2*K(1 , IN ) ) *B2*P ( 4 , 2  )  ) /SCB 
K(4, IN )  = (A2*B2*P(3,2)  +  (A4-B2*K(2, IN) ) *B2*P ( 4 , 2)  )  /  SCB 
K(5, IN)=(A1*B2*P(5,2)+(A3-B2*K(1 , IN ) ) *B2*P ( 6 , 2 )  ) /SC B 
K(6, IN)=(A2*B2*P(5,2)+(A4-B2*K(2, IN ) ) *32*P ( 5 , 2 ) ) /SCB 
CALL  SETA ( A ) 

INT=IN 

CALL  C ALA ( A , K , INT) 

CALL  MATTNS ( A , AT , 6 , 6 ) 

C 

C  U-D  FACTORIZATION  METHOD 

C 

C 

CALL  MATMUL ( P , B , V , 6 , 6 ,  1) 

CALL  MATMUL ( AT , V , V , 6 , 6 ,  1  ) 

CALL  MATMUL (AT , U , WP , 6 , 6 , 6 ) 

DO  20  1=1,6 
DB(I,I)=D(I,I) 

DO  20  J=1 , 6 

20  W(I, J)=WP(I, J) 

C 

DO  80  J J=6 , 1 , -1 
DO  65  KL  =  1  ,  12 
65  RWD(1,KL)sW(JJ,KL) 

CALL  MATTNS (RWD, TRWD , 1,12) 

CALL  MATMUL (DB , TRWD, TRWD, 12,12,1) 

CALL  MATMUL (RWD , TRWD,D(JJ,JJ) ,1,12,1) 

IF  (  D ( J  J , J  J ) .EQ.O. )  GO  TO  80 
C 

DO  80  KK= 1 , JJ-1 
DO  85  LN  =  1 ,  12 
85  RWD ( 1 , LN ) =W ( KK , LN ) 

C 

CALL  MATMUL (RWD, TRWD, US , 1,12,1) 

U(KK,JJ)=US/D(JJ,JJ) 

C 

DO  95  LK  =  1 ,  12 

95  W ( KK , LK ) sW ( KK , LK ) -U (KK , J  J ) *W( J  J , LK) 

C 

80  CONTINUE 

C 

c 

CALL  MATTNS  ( U , UT , 6 , 6 ) 

CALL  MATMUL(D,UT,UT,6,6,6) 


oooooooooooooooooooo  — *  o  oo-»  oo  o— *  -*>  oooo 
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CALL  MATMUL (U , UT , UT , 6 , 6 , 6  ) 


RANK  ONE  UPDATE 

CALL  MAUD  (UT  ,  6  ) 

DO  120  J K  =  6 , 2 , - 1 

DT(JK,JK)=D(JK,JK)+SCA*V(JK, 1 )  *V ( JK , 1 ) 

DO  110  LK= 1 , JK-1 

V(LK, 1 )=V(LK, 1 )-V(JK, 1 )*U(LK, JK) 

10  UT (LK , JK) =U (LK,JK)+SCA*V(JK, 1 )»V(LK, 1 )/DT(JK, JK) 

SCA=SCA*D(JK,JK)/DT(JK,JK) 

20  CONTINUE 

DT  ( 1 ,  1  )=D(1 , 1 )+SCA*V(1 ,  1  )  *V ( 1 , 1  ) 


DO  130  1=1,6 
D  ( I ,  I )  =DT (1,1) 
DO  130  J=1 , 6 
30  U ( I , J ) =UT ( I , J  ) 


CALL  MATTNS  (0,UT,6,6) 

CALL  MATMUL (D,UT,UT, 6, 6, 6) 
CALL  MATMUL (U,UT,UT, 6, 6,6) 

60  CONTINUE 
RETURN 
END 


SUBROUTINE  FOR  MAGNETIC  3ALL  SUSPENSION 
EXTENDED  KALMAN  FILTERING 

KF  IS  THE  KALMAN  FILTER  GAINS,  AUX  IS  THE  3-VECTOR 
FOR  THE  AUGMENTED  EIGTH  ORDER  SYSTEM,  Z  IS  THE 
MEASUREMENT  VECTOR,  K  IS  THE  CONTROL  FEEDBACK 
GAINS,  R  IS  THE  CONTROL  WEIGHT  IN  THE  COST 
EQUATION,  H  IS  THE  OBSERVATION  MATRIX,  VNW  IS  THE 
NOISE  VARIANCE  OF  THE  SYSTEM’S  NOISE,  AU  IS  THE 
8*3  SYSTEM  MATRIX,  XN  IS  THE  NOMINAL  TRAJECTORY 
FOR  THE  KALMAN  FILTER. 


SUBROUTINE  KALMAN (KF , AUX , UT , DT , Z , K , R , H , VNW , AU , XN ) 
REAL  AUX ( 3 , 1 ) ,  XN(6,  1  ) 

REAL  U ( 3 , 3 ) ,  D ( 3 , 3 ) ,  UT ( 3 , 8 ) ,  DT(3,3),  Q(3,3) 

REAL  RH ( 1 ,8),  CKF(3, 1) 

REAL  KF (  3 , 6 )  ,  Z ( 6 , 1) 
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REAL  AU(3, 8)  ,  A(6 , 6)  ,  K ( 6 , 20) 

REAL  R ( 6 , 6 )  ,  H (6 , 8 ) 

REAL  F ( 3 , 1 ) ,  DB ( 9 , 9 ) ,  WD(3,9),  RWD(1,9) 

R  F  AI  FT  M  R  1 

REAL  W P  ( 8  8 )  ,  TRWD  ( 9  ,  1  ) 

COMMON  AI , A2, A3, AU, 32, RA3, RB2 

n 

O 

CALL  E RRSET (100) 

C 

C 

C  INITIALIZE  WD'S  G  COMPONENT 
CALL  MATSM ( WD , 0 . ,8,9) 

WD( 2 , 9 )  =  A  2 
C 
C 

C  INITIALIZE  DB 

CALL  MATSM (DB , 0 . ,9,9) 

DB( 9 , 9  )  =VNW 
C 

C  INITIALIZE  U,  D 

CALL  MATSM ( U , 0 . ,8,8) 

CALL  MATSM (D , 0 . ,8,3) 

C 

C  ASSUME  SCALAR  MEASUREMENT,  GO  THRU.  LOOP  6  TIMES 
DO  100  1=1,6 
RIsRCI, I) 

DO  11  J  K  =  1 ,8 
11  RH(1,JK)=H(I,JK) 

CALL  MATMUL(RH,UT,FT, 1 ,8,8) 

CALL  MATTNS (FT , F , 1 ,8) 

CALL  MATSM ( CKF , 0 . ,8,1) 

A  JS  1  =R I+DT ( 1 , 1 ) *F( 1 , 1 ) *F(  1  ,  1) 

D ( 1 , 1 )=RI*DT( 1 , 1 ) /A JS 1 
CKF(1 , 1 ) =DT ( 1 , 1 )  *  F ( 1 ,  1) 

U ( 1 , 1 ) =UT (1,1) 

C 

DO  40  JK=2 , 8 

A J=A  JS 1 +DT (JK,JK)*F(JK, 1 ) *F ( JK , 1 ) 

D( JK, JK)=AJS1/AJ*DT( JK, JK) 

C  IF  (D(JK, JK) .EQ.O. )  GO  TO  37 

SL  =  F( JK,  1  )/AJS1 
DO  36  LL= 1 , 8 

36  U(LL, JK)=UT(LL, JK)  -  SL*CKF(LL,1) 

r 

c 

39  DO  38  LL  =  1  ,8 

38  CKF(LL,1)=CKF(LL, 1) +DT (JK,JK)*F(JK, 1 ) *UT (LL , JK) 

C 

40  AJSIrAJ 
A  J  =  1  .  /  A  J 

CALL  MATSM ( CKF , AJ , 8 ,  1  ) 

DO  50  JL  =  1  ,8 

50  KF(JL,I)=CKF(JL, 1) 

CALL  MATMUL ( RH , AUX , SM  ,  1 , 3 ,  1  ) 

3M=Z( 1 ,  1  )-SM 


CALL  MATSMCCKr  , SM , 3,  1  ) 

CALL  MATAS ( AUX, CKF , AUX, 8, 1 , 1 ) 

r 

DO  100  LOU  =  1  ,  8 
DO  100  L IN  =  1  ,  8 
UT (LOU,LIN)=U(LOU,LIN) 
DT(LOU,LIN)=D(LOU,LIN) 

100  CONTINUE 

A  3  =  A  U  X  ( 7  ,  1  ) 

B2=AUX( 8 ,  1  ) 

C 

c 

c 

111  FORMAT ( 1 X , 8G 1 0 . 4 ) 

C 

C  MODIFIED  GRAM-5CHMIDT  TIME  UPDATE 
C 

CALL  MATSM ( AU , 0 . ,8,8) 

CALL  SETA ( A ) 

CALL  CALA( A,  K, 1 ) 

CALL  CALS ( A , K ,  1  ) 

DO  10  1=1,6 
DO  10  J  =  1  ,  6 

10  A  U ( I , J ) =  A  ( I ,  J  ) 

AU (7 , 7  )  =  1 
A  U  ( 3 , 3 )  =  1 
C 

CALL  MATMUL ( AU , AUX , AUX , 8 , 8 ,  1) 

r 
^ / 

AU ( 2 , 7  )  =XN ( 1  ,  1  ) 

AU(4,7)  =  XN(3, 1)  +  XN(4,  1) 

AU(4,8)  =  -K(3,  1  )  *XN  ( 3-,  1  )  -  K  (  4  ,  1)  *XN  (4,1) 
AU( 5 , 7  )  =  XN ( 5 , 1  )  +  X  N ( 6 ,  1) 

AU(6,3)  =  -K(5, 1 ) *XN ( 5 , 1 )-K ( 6 ,  1) *XN (5,1) 
DO  7  KL  =  1 ,6 

7  AU(2,8)=AU(2,8)-K(KL,1) *XN (KL , 1 ) 

C 

CALL  MATMUL ( AU , U , WP , 8 , 3 , 8 ) 

DO  55  JL  =  1  ,  8 
DO  55  KL  =  1,8 
WD(JL,KL)=WP(JL,KL) 

55  DB ( JL , KL ) =D ( JL , KL  ) 

DO  60  J  J  =3 ,1,-1 
DO  65  KL  =  1  ,  9 

65  R W D  ( 1  ,  KL  )  =  W D  ( J  J  ,  KL  ) 

CALL  MATTNS ( RWD, TRWD , 1,9) 

CALL  M ATMUL (DB , TRWD , TRWD ,  9 , 9 ,  1  ) 

CALL  MATMUL (RWD , TRWD , DT ( J  J , J J ) ,  1  , 9,  1  ) 
IF  (DTCJJ, JJ) .EQ.O)  GO  TO  60 
C 

DO  80  KK= 1 , J J - 1 
DO  85  LNsI , 9 

85  RWD ( 1 , LN ) =WD(KK , LN ) 

CALL  MATMUL (RWD, TRWD , US , 1,9, 1) 

UT (KK , J J )=US/DT ( J  J , J J ) 
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C 

C 

DO  95  LKsI , 9 

95  WD(KK,LK)=WD(KK,LK)-UT(KK,.JJ)*WD(JJ,LK) 

C 

30  CONTINUE 

C 

60  CONTINUE 

RETURN 
END 
C 
C 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c  ************************************ 

c 

C  *******  MAIN  PROGRAM  **#*###*# 

C 

r  a*********************************** 

C 

C  R A 3 ,  RB2  ARE  PLANT  PARAMETER  VALUES 
C  A3,  B2  ARE  THE  ESTIMATES  FOR  THE  PARAMETERS 
C 
C 

COMMON  A1,A2,A3,A4,B2,RA3,RB2 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
C 
C 

MU =5 

TIME ( 1 ) =0 . 

R  =  0 . 5 

ALsI 


XN  ( 6  1  ) 

A ( 6 , 6  )  ,  B( 6 , 1 )  ,  P ( 5 , 6 )  ,  C(6,6),  F ( 6 , 6 ) 
DV ( 6 , 1  )  ,  E(1 ,6)  ,  K ( 6 , 20 ) 

G A ( 6  ,  1),  AG ( 1 ,6),  QD (6,6) 

Z ( 6  ,  1  )  ,  RA(6, 6) 

JO ( 4  )  ,  PCO ( 6 , 6  ) 

PE (6, 6) 

JTR  ( 4 ) 

RINVC6, 6) 

AUX(  3  ,  D,  KF  (3,6)  ,  H(6,3) 

AU(3, 8) 

UT (3,8) ,  DT ( 3 , 3 ) 

TIME(IOO),  XI  (1  00 )  ,  X2d00),  UdOO) 
THI(IOO),  TH2C100) 


FT 
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C 

C  PLANT  PARAMETER  VALUES 
RA  3  =0 . 5 
R32  =  -0 . 4 

n 

C  32  IS  INITIAL  ESTIMATE  FOR  RB2 
C  A3  IS  INITIAL  ESTIMATE  FOR  RA 3 

r* 

B2  =  -0 . 42 
A  1  =  1 
A2  =  0. 01 
A3=0. 485 
C 

C  THE  Z  VECTOR  IS  THE  INITIAL  STATE  CONDITION 
C  AND  ALSO  THE  INITIAL  MEASUREMENT  FOR 
C  KALMAN  FILTER 

C 

A4  =  1 

Z ( 1 , 1 )=0. 005 
Z ( 2 ,  1  )  =  0 . 05 

n 

C  VW  IS  INITIAL  VARIANCE  OF  STATE  FOR  FILTER 

n 

VW=ABS(Z(2 , 1 )/10.  ) 

Z( 3 ,  1 )  =  0 . 

Z ( 4 , 1 ) =0 . 

z ( 5 , 1  )  =  0 . 

Z ( 6 , 1 )=0. 

TYPE  1  1  1 
TYPE  1 

1  F0RMATC2X, ' INITIAL  CONDITIONS 

TYPE  111,  Z 
TYPE  111 

r 

C  INITIALIZE  THE  MATRIX  H,  U-D  FACTOR  FOR 
C  INITIAL  COVARIANCE  FOR  THE  FILTER 
C 

DO  16  1=1,8 

A  U  X ( 1 , 1 )=0 

DO  16  J  =  1 , 3 

IF  (I.LE.6)  H(I,J)=0 

IF  ((I.LE.6).AND.(I.EQ.J))  H ( I  ,  J  )  =  1  - 

DT  ( I  ,  J  )  =  0 

IF  (I.EQ. J)  UT ( I ,  J )  =  1  . 

IF  (I.EQ.J)  DT ( I , J ) = VW 
IF  ( (I.EQ. J) .AND. (I.GE.7)  )  DT(I,J)=0.1 
16  CONTINUE 

C  VNW  IS  VARIANCE  OF  NOISE 
VNW=0. 01 
C 

AUX(7 ,  1  )=A3 
A  U  X ( 3 ,  1 )=B2 

r 


DO  2  L  =  1  ,  6 


ooo  o  woons  ooo  in  <n  ooooo  r-  o  *-  o  o  o  i>  t>  •■—  o  o 
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2  B(L ,  1  )  =0 

B ( 2 , 1  )  =32 

TO  GENERATE  FILE  FOR  PLOTTING 
TYPE  121 

21  FORM AT ( '  INPUT  FILE  NUMBER  FOR  GENERATING  GRAPH 

1  IN  12  FORMAT ' ) 

ACCEPT  22,  NF 
2  FORMAT  (12) 


TYPE  4 

FORMAT ( '  INPUT  WNO ’ ) 


INPUT  MAXIMUM  WEIGHT  FOR  MATRIX  W 


ACCEPT  5,  WNO 
FORMAT (F 1 0 . 4) 

IF  (WNO.GT.1.)  STOP 
TYPE  6,  WNO 

FORMAT ( /  ,  '  WNO  =  ' ,F 1 0. 3,/) 

INWsO 


INITIALIZE  NOISE  MATRIX,  STATE  WEIGHTING  MATRIX, 
NOMINAL  TRAJECTORY  OF  FILTER,  AND  NOISE 
VARIANCE  FOR  OBSERVATION:  RINV 

DO  7  M  =  1  ,  6 
XN(M,1)=Z(M,1) 

GA ( M , 1 ) =0 

IF (M . EQ. 2)  GA ( M , 1 ) =A2 
DO  7  N  =  1  ,  6 
QD (M , N ) =0 
RINVCM, N)=0 

IF  (M.EQ.N)  RINVCM, N ) sVNW/ 1 00000 . 

CONTINUE 
QD( 1 , 1 )  =  4  00 
QD (2 , 2 ) =0 . 2 

ICOUNT  =  1 

00  DO  12  KINDsI , 4 
KINDT  =KIND 

CALL  SWQD (QD, WNO, KINDT, AL) 

COMPUTATION  OF  K,  THE  FEEDBACK  GAINS 
CALL  SETA ( A ) 

CALL  COMK'JD  (  A  ,  B  ,  QD  ,  K  ,  R  ,  MU  ) 


11  FORM AT ( 1 X , 8G 1 0 . 4 ) 

COMPUTATION  OF  OPTIMAL  COST 
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CALL  COMPJCZ, A,K,QD,GA, VNW, JO (KIND) ,MU) 

12  CONTINUE 
TYPE  13 

13  FORMAT(2X, 'OPTIMAL  COST  FOR  DIFFERENT  WNO 
1  POSITION',/) 

TYPE  111,  JO 
TYPE  111 
ISW  =  4 
S JO  =  JO  (4 ) 

DO  10  1=3, 1 , - 1 

IF  ( S JO . LE . JO (I ) )  GO  TO  10 

3 JO  =  JO  (I ) 

ISW  =  I 

10  CONTINUE 
TYPE  111,  ISW 

IF  (ISW.EQ.4)  GO  TO  11 
CALL  SWQDCQD, WNO, ISW, AL) 

CALL  SETA ( A ) 

CALL  COMKUD ( A , B , QD , K , R , MU ) 

C 

c 

c 

C  TO  GENERATE  DATA  FOR  PLOTTING 
C 

11  XI (ICOUNT)=Z( 1  ,  1  ) 

X2 ( ICOUNT ) =Z ( 2  , 1) 

TH 1 ( ICOUNT ) =A  3 
TH2 ( ICOUNT ) =B2 
DRA3=RA3-A3 
DRB2=R32-B2 

WRITE (NF  ,202)  TIME (ICOUNT) , XI (ICOUNT) ,X2( ICOUNT) 
NF 1 =NF  + 1 

WRITE (NF1 ,202)  TIME (ICOUNT) , DR A3 , TH 1 (ICOUNT)  , 

1  DR32 ,  TH2 ( ICOUNT  ) 

202  F0RMAT(5(1X,G12.4)  ) 

C 

C 

C 

C  KALMAN  FILTER  TO  OBTAIN  ESTIMATES  A3,  32 
C 

CALL  KALMAN (KF , AUX, UT , DT , Z , K , RINV , H , VNW , AU , XN ) 
CALL  WDSK(AU, 3, 3, 4) 

TYPE  111,  AUX 

TYPE  111,  A3,  32,  ICOUNT 

CALL  SETA(A) 

CALL  C ALA ( A , K ,  1  ) 

CALL  CALB( A, K, 1  ) 

CALL  MATMUL ( A , XN , XN , 6 , 6 ,  1 ) 

r 

C  COMPUTE  NEXT  STATE  USING  PLANT  EQUATION 

C 

C 

CALL  SETRA(RA) 

CALL  C ALRA( R A , K ,  1  ) 

CALL  C ALRB( R A , K ,  1  ) 
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WRITE  A  MATRIX  ON  DISK  FOR  LAS  ANALYSIS 
CALL  WDSKCRA ,6,6,3) 


TYPE  111,  (K(JJ, 1)  ,  J J  =  1 ,6) 

DO  15  JK= 1 , 6 
5  E(1 , JK)=K(JK, 1 ) 

CALL  MATMUL (E ,  Z  ,  U  ( ICOUNT ) , 1 , 6 , 1 ) 

TYPE  111,  U (ICOUNT ) 

GENERATE  DATA  FOR  PLOTTING 
NF2=NF  +2 

WRITE(NF2, 202)  TIME ( ICOUNT ) ,  U(ICOUNT) 

CALL  MATMUL (RA,Z,Z,6,6, 1) 

INW=INW+1 

CALL  R ANDOM ( INW , VNW , 0 . , W , KKK ) 

TYPE  111,  W 
DO  17  JL= 1 , 6 

7  DV(JL, 1 ) =GA ( JL , 1 )*W 

CALL  MATAS ( Z , DV , Z , 6 , 1,1) 

TYPE  111,  Z 


ACOUNT  =ICOUNT 

THE  FOLLOWING  STATEMENTS  WITH  COMMENT  ON 
FIRST  COLUMN  ARE  FOR  CHANGING  PARAMETER 
AT  T  =0 . 2  AND  PERTURBATION  OF  STATES  AT 
THAT  INSTANCE 


IF  (ICOUNT. EQ. 20)  RA3=0.4 
IF  (ICOUNT. EQ. 20)  RB2=-0.3 
IF  (ICOUNT. EQ. 20)  Z( 1 , 1 ) =Z ( 1 , 1 )+0 . 005 
IF  (ICOUNT. EQ. 20)  Z ( 2 ,  1 ) =Z ( 2 , 1 )  +  0 . 0 1 

IF  (ICOUNT. EQ. 41)  STOP 

TIME(ICOUNT  +  1)=  A2  +  TIME(ICOUNT) 

ICOUNT  =  IC0UNT  +  1 

GO  TO  100 

END 


